changes

README.md

@@ -4,7 +4,7 @@
 
 This is the ROS driver for the Roboclaw motor controllers made by [Ion Motion Control](http://www.ionmc.com/).
 
-#HELP: I have been busy with another project that is not using robo claw. Message me if you want to become a contributer and help keep this thing alive!
+#HELP: I have been busy with another project that is not using robo claw. Message me if you want to become a contributor and help keep this thing alive!
 
 ## Before you begin
 Before you use this package you need to calibrate the velocity PID on the Roboclaw.  This will requare the
@@ -33,7 +33,7 @@ be manually set for the motor before starting. Autotune functions usually return
 values but in most cases you will still need to manually adjust them for optimum performance.
 
 ## Usage
-Just clone the repo into your catkin workspace. It contains the ROS package and the motor controller driver.  Remmeber to make sure ROS has permisions to use the dev port you give it.
+Just clone the repo into your catkin workspace. It contains the ROS package and the motor controller driver.  Remember to make sure ROS has permissions to use the dev port you give it.
 ```bash
 cd <workspace>/src
 git clone https://github.com/sonyccd/roboclaw_ros.git
@@ -54,11 +54,14 @@ The launch file can be configure at the command line with arguments, by changing
 |max_speed|2.0|Max speed allowed for motors in meters per second|
 |ticks_per_meter|4342.2|The number of encoder ticks per meter of movement|
 |base_width|0.315|Width from one wheel edge to another in meters|
+|pub_odom|true|Publishes Odometry if set to true|
+|stop_movement|true|Stops movement if no velocity commands are received for 1 sec|
 
 ## Topics
 ###Subscribed
 /cmd_vel [(geometry_msgs/Twist)](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html)  
 Velocity commands for the mobile base.
+
 ###Published
 /odom [(nav_msgs/Odometry)](http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html)  
 Odometry output from the mobile base.

roboclaw_node/launch/roboclaw.launch

@@ -8,14 +8,19 @@
     <arg name="ticks_per_meter" default="4342.2"/>
     <arg name="base_width" default="0.315"/>
     <arg name="run_diag" default="true"/>
+    <arg name="pub_odom" default="true"/>
+    <arg name="stop_movement" default="true"/>
 
-    <node if="$(arg run_diag)" pkg="roboclaw_node" type="roboclaw_node.py" name="roboclaw_node">
+
+    <node if="$(arg run_diag)" pkg="roboclaw_node" type="roboclaw_node.py" name="roboclaw_node" output="screen">
         <param name="~dev" value="$(arg dev)"/>
         <param name="~baud" value="$(arg baud)"/>
         <param name="~address" value="$(arg address)"/>
         <param name="~max_speed" value="$(arg max_speed)"/>
         <param name="~ticks_per_meter" value="$(arg ticks_per_meter)"/>
         <param name="~base_width" value="$(arg base_width)"/>
+        <param name="~pub_odom" value="$(arg pub_odom)"/>
+        <param name="~stop_movement" value="$(arg stop_movement)"/>
     </node>
 
     <node pkg="diagnostic_aggregator" type="aggregator_node"

roboclaw_node/nodes/roboclaw_node.py

@@ -11,20 +11,20 @@
 
 __author__ = "[email protected] (Brad Bazemore)"
 
-
 # TODO need to find some better was of handling OSerror 11 or preventing it, any ideas?
 
 class EncoderOdom:
     def __init__(self, ticks_per_meter, base_width):
         self.TICKS_PER_METER = ticks_per_meter
         self.BASE_WIDTH = base_width
-        self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
+        self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=1)
         self.cur_x = 0
         self.cur_y = 0
         self.cur_theta = 0.0
         self.last_enc_left = 0
         self.last_enc_right = 0
         self.last_enc_time = rospy.Time.now()
+        self.vel_theta = 0
 
     @staticmethod
     def normalize_angle(angle):
@@ -67,6 +67,7 @@ def update(self, enc_left, enc_right):
             vel_x = dist / d_time
             vel_theta = d_theta / d_time
 
+        self.vel_theta = vel_theta
         return vel_x, vel_theta
 
     def update_publish(self, enc_left, enc_right):
@@ -115,6 +116,64 @@ def publish_odom(self, cur_x, cur_y, cur_theta, vx, vth):
 
         self.odom_pub.publish(odom)
 
+class Movement:
+    def __init__(self, address, max_speed, base_width, ticks_per_meter):
+        self.twist = None
+        self.address = address
+        self.MAX_SPEED = max_speed
+        self.BASE_WIDTH = base_width
+        self.TICKS_PER_METER = ticks_per_meter
+        self.last_set_speed_time = rospy.get_rostime()
+        self.vr_ticks = 0
+        self.vl_ticks = 0
+        self.stopped = True
+
+    def run(self):
+        if self.twist is None:
+            return
+        # self.last_set_speed_time = rospy.get_rostime()
+
+        if self.twist.linear.x != 0 or self.twist.angular.z != 0:
+            self.stopped = False
+
+        linear_x = self.twist.linear.x
+        if linear_x > self.MAX_SPEED:
+            linear_x = self.MAX_SPEED
+        if linear_x < -self.MAX_SPEED:
+            linear_x = -self.MAX_SPEED
+
+        vr = linear_x + self.twist.angular.z * self.BASE_WIDTH  # m/s
+        vl = linear_x - self.twist.angular.z * self.BASE_WIDTH
+        self.twist = None
+
+        vr_ticks = int(vr * self.TICKS_PER_METER)  # ticks/s
+        vl_ticks = int(vl * self.TICKS_PER_METER)
+
+        #print("-- vr_ticks:{} vl_ticks:{}".format(vr_ticks, vl_ticks))
+        rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
+
+        try:
+            # This is a hack way to keep a poorly tuned PID from making noise at speed 0
+
+            # if vr_ticks is 0 and vl_ticks is 0:
+            #     roboclaw.ForwardM1(self.address, 0)
+            #     roboclaw.ForwardM2(self.address, 0)
+            # else:
+            #     roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks)
+
+            if vr_ticks is 0 and vl_ticks is 0:
+                roboclaw.ForwardM1(self.address, 0)
+                roboclaw.ForwardM2(self.address, 0)
+                self.vr_ticks = 0
+                self.vl_ticks = 0
+            else:
+                gain = 0.5
+                self.vr_ticks = gain * vr_ticks + (1 - gain) * self.vr_ticks
+                self.vl_ticks = gain * vl_ticks + (1 - gain) * self.vl_ticks
+                roboclaw.SpeedM1M2(self.address, int(self.vr_ticks), int(self.vl_ticks))
+        except OSError as e:
+            rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
+            rospy.logdebug(e)
 
 class Node:
     def __init__(self):
@@ -177,13 +236,18 @@ def __init__(self):
         roboclaw.ResetEncoders(self.address)
 
         self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0"))
-        self.TICKS_PER_METER = float(rospy.get_param("~tick_per_meter", "4342.2"))
+        self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter", "4342.2"))
         self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315"))
+        self.PUB_ODOM = rospy.get_param("~pub_odom", "true")
+        self.STOP_MOVEMENT = rospy.get_param("~stop_movement", "true")
 
-        self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
+        self.encodm = None
+        if (self.PUB_ODOM):
+            self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
+        self.movement = Movement(self.address, self.MAX_SPEED, self.BASE_WIDTH, self.TICKS_PER_METER)
         self.last_set_speed_time = rospy.get_rostime()
 
-        rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
+        rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=1)
 
         rospy.sleep(1)
 
@@ -196,17 +260,19 @@ def __init__(self):
 
     def run(self):
         rospy.loginfo("Starting motor drive")
-        r_time = rospy.Rate(10)
+        r_time = rospy.Rate(30)
         while not rospy.is_shutdown():
 
-            if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1:
+            # stop movement if robot doesn't recieve commands for 1 sec
+            if (self.STOP_MOVEMENT and not self.movement.stopped and rospy.get_rostime().to_sec() - self.movement.last_set_speed_time.to_sec() > 1):
                 rospy.loginfo("Did not get command for 1 second, stopping")
                 try:
                     roboclaw.ForwardM1(self.address, 0)
                     roboclaw.ForwardM2(self.address, 0)
                 except OSError as e:
                     rospy.logerr("Could not stop")
                     rospy.logdebug(e)
+                self.movement.stopped = True
 
             # TODO need find solution to the OSError11 looks like sync problem with serial
             status1, enc1, crc1 = None, None, None
@@ -228,40 +294,17 @@ def run(self):
                 rospy.logwarn("ReadEncM2 OSError: %d", e.errno)
                 rospy.logdebug(e)
 
-            if ('enc1' in vars()) and ('enc2' in vars()):
+            if ('enc1' in vars()) and ('enc2' in vars() and enc1 and enc2):
                 rospy.logdebug(" Encoders %d %d" % (enc1, enc2))
-                self.encodm.update_publish(enc1, enc2)
-
+                if (self.encodm):
+                    self.encodm.update_publish(enc1, enc2)
                 self.updater.update()
+            self.movement.run()
             r_time.sleep()
 
     def cmd_vel_callback(self, twist):
-        self.last_set_speed_time = rospy.get_rostime()
-
-        linear_x = twist.linear.x
-        if linear_x > self.MAX_SPEED:
-            linear_x = self.MAX_SPEED
-        if linear_x < -self.MAX_SPEED:
-            linear_x = -self.MAX_SPEED
-
-        vr = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0  # m/s
-        vl = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0
-
-        vr_ticks = int(vr * self.TICKS_PER_METER)  # ticks/s
-        vl_ticks = int(vl * self.TICKS_PER_METER)
-
-        rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks)
-
-        try:
-            # This is a hack way to keep a poorly tuned PID from making noise at speed 0
-            if vr_ticks is 0 and vl_ticks is 0:
-                roboclaw.ForwardM1(self.address, 0)
-                roboclaw.ForwardM2(self.address, 0)
-            else:
-                roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks)
-        except OSError as e:
-            rospy.logwarn("SpeedM1M2 OSError: %d", e.errno)
-            rospy.logdebug(e)
+        self.movement.last_set_speed_time = rospy.get_rostime()
+        self.movement.twist = twist
 
     # TODO: Need to make this work when more than one error is raised
     def check_vitals(self, stat):