Scoot refactor, Drive & joint movment blocking, overshoot turn fix

src/drive_controller/src/drive_controller.py

@@ -10,6 +10,7 @@
 from std_msgs.msg import Float64
 
 M_PI_2 = 1.5708
+M_PI = M_PI_2 * 2
 
 class DriveController:
 
@@ -32,6 +33,16 @@ def __init__(self):
         self.wheel_base = 1.0
         self.steering_track = 1.0
 
+
+    def clip_steering_angle(self, steer, vel):
+        max_steer = M_PI_2
+        min_steer = -1 * M_PI_2
+        if (steer > max_steer) and (steer - M_PI > min_steer):
+            return steer - M_PI, -1 * vel
+        if (steer < min_steer) and (steer + M_PI < max_steer):
+            return steer + M_PI, -1 * vel
+        return steer, vel
+
     def _drive(self, msg):
         lin_x = msg.linear.x
         lin_y = msg.linear.y
@@ -51,6 +62,11 @@ def _drive(self, msg):
         front_right_steering = math.atan2(y_comp_plus_ang, x_comp_plus_ang)
         back_left_steering = math.atan2(y_comp_minus_ang, x_comp_minus_ang)
         back_right_steering = math.atan2(y_comp_minus_ang, x_comp_plus_ang)
+
+        front_left_steering, vel_left_front = self.clip_steering_angle(front_left_steering, vel_left_front)
+        front_right_steering, vel_right_front = self.clip_steering_angle(front_right_steering, vel_right_front)
+        back_left_steering, vel_left_back = self.clip_steering_angle(back_left_steering, vel_left_back)
+        back_right_steering, vel_right_back = self.clip_steering_angle(back_right_steering, vel_right_back)
 
         self.__drive_front_left_wheel(vel_left_front)
         self.__drive_front_right_wheel(vel_right_front)
@@ -61,7 +77,7 @@ def _drive(self, msg):
         self.__steer_front_right_wheel(front_right_steering)
         self.__steer_back_left_wheel(back_left_steering)
         self.__steer_back_right_wheel(back_right_steering)
-
+  
     def __drive_front_left_wheel(self, velocity):
         v = Float64()
         v.data = velocity

src/scoot/launch/scoot.xml

@@ -17,20 +17,20 @@
 
   <node name="Core" pkg="scoot" type="Core.py" respawn="true">
     <param name="HEADING_RESTORE_FACTOR" value="1.0"/>
-    <param name="GOAL_DISTANCE_OK" value="0.1"/>
-    <param name="ROTATE_THRESHOLD" value="0.19"/>
+    <param name="GOAL_DISTANCE_OK" value="0.05"/>
+    <param name="ROTATE_THRESHOLD" value="0.001"/>
     <param name="DRIVE_ANGLE_ABORT" value="1.5"/>
 
     <!-- Default drive speeds. These are also used for manual driving. -->
-    <param name="DRIVE_SPEED" value="5"/>
-    <param name="REVERSE_SPEED" value="3"/>
-    <param name="TURN_SPEED" value="5"/>
+    <param name="DRIVE_SPEED" value="1"/>
+    <param name="REVERSE_SPEED" value="1"/>
+    <param name="TURN_SPEED" value="1"/>
 
     <!-- Additional slow and fast drive speeds available through the Swarmie API -->
-    <param name="DRIVE_SPEED_SLOW" value="0.1"/>
+    <param name="DRIVE_SPEED_SLOW" value="0.5"/>
     <param name="TURN_SPEED_SLOW" value="0.5"/>
-    <param name="DRIVE_SPEED_FAST" value="0.25"/>
-    <param name="TURN_SPEED_FAST" value="1.1"/>
+    <param name="DRIVE_SPEED_FAST" value="2"/>
+    <param name="TURN_SPEED_FAST" value="2"/>
   </node>
 
   <node name="odom" pkg="scoot" type="FakeLocalization.py" args="$(arg name)" respawn="true" >

src/scoot/msg/MoveRequest.msg

@@ -1,4 +1,5 @@
-float64 r 
+float64 x
+float64 y
 float64 theta
 float64 timer
 float64 angular

src/scoot/src/Core.py

@@ -22,7 +22,6 @@
             rospy.loginfo("Core Starting without Task")
 
         while not rospy.is_shutdown():
-            driver.run()
             r.sleep()
     except rospy.ROSInterruptException:
         pass