Implement wheel odometry

ROS/osr/src/roboclaw_wrapper.py

@@ -60,7 +60,7 @@ def __init__(self):
     def run(self):
         """Blocking loop which runs after initialization has completed"""
         rate = rospy.Rate(8)
- 
+
         status = Status()
 
         # time last command was executed in the run loop
@@ -150,7 +150,7 @@ def establish_roboclaw_connections(self):
         # initialize connection status to successful
         all_connected = True
         for address in self.address:
-            rospy.logdebug("Attempting to talk to motor controller ''".format(address))
+            rospy.logdebug("Attempting to talk to motor controller '{}'".format(address))
             version_response = self.rc.ReadVersion(address)
             connected = bool(version_response[0])
             if not connected:

ROS/osr/src/rover.py

@@ -2,10 +2,13 @@
 
 import rospy
 import math
+import tf2_ros
 
 from sensor_msgs.msg import JointState
-from geometry_msgs.msg import Twist
+from geometry_msgs.msg import Twist, TwistWithCovariance, TransformStamped
+from nav_msgs.msg import Odometry
 from osr_msgs.msg import CommandDrive, CommandCorner
+from std_msgs.msg import Float64
 
 
 class Rover(object):
@@ -25,11 +28,22 @@ def __init__(self):
         self.wheel_radius = rospy.get_param("/rover_dimensions/wheel_radius", 0.075)  # [m]
         drive_no_load_rpm = rospy.get_param("/drive_no_load_rpm", 130)
         self.max_vel = self.wheel_radius * drive_no_load_rpm / 60 * 2 * math.pi  # [m/s]
-        self.curr_twist = Twist()
+        self.should_calculate_odom = rospy.get_param("~enable_odometry", False)
+        self.odometry = Odometry()
+        self.odometry.header.stamp = rospy.Time.now()
+        self.odometry.header.frame_id = "odom"
+        self.odometry.child_frame_id = "base_link"
+        self.odometry.pose.pose.orientation.z = 1.
+        self.odometry.pose.pose.orientation.w = 1.
+        self.curr_twist = TwistWithCovariance()
         self.curr_turning_radius = self.max_radius
 
         self.corner_cmd_pub = rospy.Publisher("/cmd_corner", CommandCorner, queue_size=1)
         self.drive_cmd_pub = rospy.Publisher("/cmd_drive", CommandDrive, queue_size=1)
+        self.turning_radius_pub = rospy.Publisher("/turning_radius", Float64, queue_size=1)
+        if self.should_calculate_odom:
+            self.odometry_pub = rospy.Publisher("/odom", Odometry, queue_size=2)
+        self.tf_pub = tf2_ros.TransformBroadcaster()
 
         rospy.Subscriber("/cmd_vel", Twist, self.cmd_cb, callback_args=False)
         rospy.Subscriber("/cmd_vel_intuitive", Twist, self.cmd_cb, callback_args=True)
@@ -74,7 +88,35 @@ def cmd_cb(self, twist_msg, intuitive=False):
     def enc_cb(self, msg):
         self.curr_positions = dict(zip(msg.name, msg.position))
         self.curr_velocities = dict(zip(msg.name, msg.velocity))
-        self.forward_kinematics()
+        if self.should_calculate_odom:
+            # measure how much time has elapsed since our last update
+            now = rospy.Time.now()
+            dt = (now - self.odometry.header.stamp).to_sec()
+            self.forward_kinematics()
+            dx = self.curr_twist.twist.linear.x * dt
+            dth = self.curr_twist.twist.angular.z * dt
+            # angle is straightforward: in 2D it's additive
+            # first calculate the current_angle in the fixed frame
+            current_angle = 2 * math.atan2(self.odometry.pose.pose.orientation.z, 
+                                           self.odometry.pose.pose.orientation.w)
+            new_angle = current_angle + dth
+            self.odometry.pose.pose.orientation.z = math.sin(new_angle/2.)
+            self.odometry.pose.pose.orientation.w = math.cos(new_angle/2.)
+            # the new pose in x and y depends on the current heading
+            self.odometry.pose.pose.position.x += math.cos(new_angle) * dx
+            self.odometry.pose.pose.position.y += math.sin(new_angle) * dx
+            self.odometry.pose.covariance = 36 * [0.0,]
+            self.odometry.twist = self.curr_twist
+            self.odometry.header.stamp = now
+            self.odometry_pub.publish(self.odometry)
+            transform_msg = TransformStamped()
+            transform_msg.header.frame_id = "odom"
+            transform_msg.child_frame_id = "base_link"
+            transform_msg.header.stamp = now
+            transform_msg.transform.translation.x = self.odometry.pose.pose.position.x
+            transform_msg.transform.translation.y = self.odometry.pose.pose.position.y
+            transform_msg.transform.rotation = self.odometry.pose.pose.orientation
+            self.tf_pub.sendTransform(transform_msg)
 
     def corner_cmd_threshold(self, corner_cmd):
         try:
@@ -272,6 +314,14 @@ def forward_kinematics(self):
         rospy.logdebug_throttle(1, "Current approximate turning radius: {}".format(round(approx_turning_radius, 2)))
         self.curr_turning_radius = approx_turning_radius
 
+        # we know that the linear velocity in x direction is the instantaneous velocity of the middle virtual
+        # wheel which spins at the average speed of the two middle outer wheels.
+        drive_angular_velocity = (self.curr_velocities['drive_left_middle'] + self.curr_velocities['drive_right_middle']) / 2.
+        self.curr_twist.twist.linear.x = drive_angular_velocity * self.wheel_radius
+        # now calculate angular velocity from its relation with linear velocity and turning radius
+        self.curr_twist.twist.angular.z = self.curr_twist.twist.linear.x / self.curr_turning_radius
+        # covariance
+        self.curr_twist.covariance = 36 * [0.0,]
 
 if __name__ == '__main__':
     rospy.init_node('rover', log_level=rospy.INFO)

ROS/osr_bringup/launch/osr.launch

@@ -1,4 +1,7 @@
 <launch>
+	<!-- arguments -->
+	<arg name="enable_odometry" default="false" doc="calculate and publish odometry to the /odom topic and to /tf"/>
+
 	<rosparam file="$(find osr_bringup)/config/osr_params.yaml" />
 
 	<!-- Nodes to run the Open Source Rover -->
@@ -18,7 +21,9 @@
         </node>
 	<rosparam command="load" file="$(find osr_bringup)/config/physical_properties.yaml"/>
 	<rosparam command="load" file="$(find osr_bringup)/config/physical_properties_mod.yaml"/>
-	<node name="rover" pkg="osr" type="rover.py" output="screen"/>
+	<node name="rover" pkg="osr" type="rover.py" output="screen">
+		<param name="enable_odometry" value="$(arg enable_odometry)"/>
+	</node>
 	<node name="led_screen" pkg="led_screen" type="arduino_comm.py"/>
 	<node pkg="joy"
 			type="joy_node" name="joy_node">

setup/rover_bringup.md

@@ -16,6 +16,12 @@ command the rover by holding the left back button (LB) down and moving the joyst
 the [RPi setup](rpi.md) by holding down the right back button (RB) instead. If this isn't working for you, 
 `rostopic echo /joy`, press buttons, and adjust `bringup.launch` to point to the corresponding buttons and axes. If you have questions, please ask on the Tapatalk forum.
 
+### Optional arguments
+
+If you want the code to calculate and publish wheel odometry, launch with the argument `enable_odometry:=true`.
+![](wheel_odom_example.png)
+Odometry is used for localization and SLAM.
+
 ## 2 Custom osr_mod.launch file
 
 If you want to customize your `osr.launch` file, make a copy of it in the same directory (`osr-rover-code/ROS/osr_bringup/launch/`) and name it `osr_mod.launch`. The OSR launch script will automatically find it.