Merge pull request #132 from Achllle/backport_odom_foxy

Achllle · web-flow · commit 980eb0838541 · 2021-05-19T20:40:28.000-07:00
Backport odometry to foxy-devel and other ROS2 related fixes
diff --git a/.gitignore b/.gitignore
@@ -1,3 +1,5 @@
 *.pyc
 .vscode
 ROS/osr_bringup/config/*_mod.yaml
+*.sublime-project
+*.sublime-workspace
diff --git a/ROS/README.md b/ROS/README.md
@@ -5,8 +5,8 @@
 There are 4 catkin packages contained in this repo. Each of these packages performs a specific purpose in the ROS 
 structure, which are covered below
 
-  * `osr`: core code that talks to motor drivers and listens to commands 
-  * `osr_msgs`: custom message definitions
+  * `osr_control`: core code that talks to motor drivers and listens to commands 
+  * `osr_interfaces`: custom message definitions
   * `osr_bringup`: configuration and launch files for starting the rover
   * `led_screen`: code to communicate to the Arduino Uno to run the LED screen
 
diff --git a/ROS/osr_bringup/launch/.gitignore b/ROS/osr_bringup/launch/.gitignore
@@ -0,0 +1 @@
+osr_mod.launch
diff --git a/ROS/osr_bringup/launch/osr_launch.py b/ROS/osr_bringup/launch/osr_launch.py
@@ -1,6 +1,8 @@
 import os
 
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
 
@@ -18,23 +20,33 @@ def generate_launch_description():
         'osr_params.yaml'
     )
 
-    return LaunchDescription([
+    ld = LaunchDescription()
+    
+    ld.add_action(
         Node(
             package='osr_control',
             executable='roboclaw_wrapper',
             name='roboclaw_wrapper',
             output='screen',
             emulate_tty=True,
             parameters=[roboclaw_params]
-        ),
+        )
+    )
+    ld.add_action(
+        DeclareLaunchArgument('enable_odometry', default_value='false')
+    )
+    ld.add_action(
         Node(
             package='osr_control',
             executable='rover',
             name='rover',
             output='screen',
             emulate_tty=True,
-            parameters=[osr_params]
-        ),
+            parameters=[osr_params,
+                        {'enable_odometry': LaunchConfiguration('enable_odometry')}]
+        )
+    )
+    ld.add_action(
         Node(
             package='teleop_twist_joy',
             executable='teleop_node',
@@ -54,7 +66,9 @@ def generate_launch_description():
             remappings=[
                 ('/cmd_vel', '/cmd_vel_intuitive')
             ]
-        ),
+        )
+    )
+    ld.add_action(
         Node(
             package='joy',
             executable='joy_node',
@@ -64,5 +78,8 @@ def generate_launch_description():
             parameters=[
                 {"autorepeat_rate": 5.0},
                 {"device_id": 0},  # This might be different on your computer. Run `ls -l /dev/input/js0`. If you have js1, put 1.
-            ]        )
-    ])
+            ]        
+        )
+    )
+
+    return ld
diff --git a/ROS/osr_control/osr_control/roboclaw_wrapper.py b/ROS/osr_control/osr_control/roboclaw_wrapper.py
@@ -159,36 +159,56 @@ def __init__(self):
         self.status = Status()
         fast_loop_rate = 0.125  # seconds
         slow_loop_rate = 3  # seconds
+        # true if we're idling and started rapping down velocity to bring the motors to full stop
+        self.idle_ramp = False
+        # if we're idled
+        self.idle = False
         self.fast_timer = self.create_timer(fast_loop_rate, self.fast_update)
         self.slow_timer = self.create_timer(slow_loop_rate, self.slow_update)
 
     def fast_update(self):
         """Read from and write to roboclaws"""
         # Check to see if there are commands in the buffer to send to the motor controller
+        now = self.get_clock().now()
         if self.drive_cmd_buffer:
             drive_fcn = self.send_drive_buffer_velocity
             drive_fcn(self.drive_cmd_buffer)
             self.drive_cmd_buffer = None
+            self.idle_ramp = False
+            self.idle = False
+            self.time_last_cmd = now
             
         if self.corner_cmd_buffer:
             self.send_corner_buffer(self.corner_cmd_buffer)
             self.corner_cmd_buffer = None
+            self.idle_ramp = False
+            self.idle = False
+            self.time_last_cmd = now
 
         # read from roboclaws and publish
         try:
             self.read_encoder_values()
             self.enc_pub.publish(self.current_enc_vals)
         except AssertionError as read_exc:
-            self.get_logger().warn( "Failed to read encoder values")
+            self.get_logger().warn("Failed to read encoder values")
 
         # stop the motors if we haven't received a command in a while
-        # this should be a timer instead
-        now = self.get_clock().now()
-        if now - self.time_last_cmd > self.velocity_timeout:
-            # rather than a hard stop, send a ramped velocity command
-            self.drive_cmd_buffer = CommandDrive()
-            self.send_drive_buffer_velocity(self.drive_cmd_buffer)
-            self.time_last_cmd = now  # so this doesn't get called all the time
+        if not self.idle and (now - self.time_last_cmd > self.velocity_timeout):
+            # rather than a hard stop, send a ramped velocity command to 0
+            if not self.idle_ramp:
+                self.get_logger().debug("Idling: ramping down velocity to zero")
+                self.idle_ramp = True
+                drive_cmd_buffer = CommandDrive()
+                self.send_drive_buffer_velocity(drive_cmd_buffer)
+            # if we've already ramped down, send a full stop to minimize
+            # idle power consumption
+            else:
+                self.get_logger().debug("Idling: full stopping motors")
+                self.stop_motors()
+                self.idle = True
+            
+            # so that's there's a delay between ramping and full stop
+            self.time_last_cmd = now
 
     def slow_update(self):
         """Slower roboclaw read/write cycle"""
@@ -263,7 +283,6 @@ def corner_cmd_cb(self, cmd):
         on the next iteration of the run() loop.
         """
         self.get_logger().debug("Corner command callback received: {}".format(cmd))
-        self.time_last_cmd = self.get_clock().now()
         self.corner_cmd_buffer = cmd
 
     def send_corner_buffer(self, cmd):
@@ -309,7 +328,6 @@ def drive_cmd_cb(self, cmd):
         
         self.get_logger().debug("Drive command callback received: {}".format(cmd))
         self.drive_cmd_buffer = cmd
-        self.time_last_cmd = self.get_clock().now()
 
     def send_drive_buffer_velocity(self, cmd):
         """
diff --git a/ROS/osr_control/osr_control/rover.py b/ROS/osr_control/osr_control/rover.py
@@ -3,9 +3,12 @@
 
 import rclpy
 from rclpy.node import Node
+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 std_msgs.msg import Float64
 from osr_interfaces.msg import CommandDrive, CommandCorner
 
 
@@ -24,7 +27,8 @@ def __init__(self):
                 ('rover_dimensions.d3', None),
                 ('rover_dimensions.d4', None),
                 ('rover_dimensions.wheel_radius', None),
-                ('drive_no_load_rpm', None)
+                ('drive_no_load_rpm', None),
+                ('enable_odometry', None)
             ]
         )
         self.d1 = self.get_parameter('rover_dimensions.d1').get_parameter_value().double_value
@@ -41,7 +45,16 @@ def __init__(self):
         drive_no_load_rpm = self.get_parameter(
             "drive_no_load_rpm").get_parameter_value().double_value
         self.max_vel = self.wheel_radius * drive_no_load_rpm / 60 * 2 * math.pi  # [m/s]
-        self.curr_twist = Twist()
+        self.should_calculate_odom = self.get_parameter("enable_odometry").get_parameter_value().bool_value
+        if self.should_calculate_odom:
+            self.get_logger().info("Calculting wheel odometry and publishing to /odom topic")
+            self.odometry = Odometry()
+            self.odometry.header.stamp = self.get_clock().now().to_msg()
+            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.cmd_vel_sub = self.create_subscription(Twist, "/cmd_vel", 
@@ -50,6 +63,11 @@ def __init__(self):
                                                         partial(self.cmd_cb, intuitive=True), 1)
         self.encoder_sub = self.create_subscription(JointState, "/encoder", self.enc_cb, 1)
 
+        self.turning_radius_pub = self.create_publisher(Float64, "/turning_radius", 1)
+        if self.should_calculate_odom:
+            self.odometry_pub = self.create_publisher(Odometry, "/odom", 2)
+            self.tf_pub = tf2_ros.TransformBroadcaster(self)
+
         self.corner_cmd_pub = self.create_publisher(CommandCorner, "/cmd_corner", 1)
         self.drive_cmd_pub = self.create_publisher(CommandDrive, "/cmd_drive", 1)
 
@@ -94,7 +112,40 @@ 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 = self.get_clock().now()
+            dt = float(now.nanoseconds - (self.odometry.header.stamp.sec*10**9 + self.odometry.header.stamp.nanosec)) / 10**9
+            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,]
+            # explanation for values at https://www.freedomrobotics.ai/blog/tuning-odometry-for-wheeled-robots
+            self.odometry.pose.covariance[0] = 0.0225
+            self.odometry.pose.covariance[5] = 0.01
+            self.odometry.pose.covariance[-5] = 0.0225
+            self.odometry.pose.covariance[-1] = 0.04
+            self.odometry.twist = self.curr_twist
+            self.odometry.header.stamp = now.to_msg()
+            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.to_msg()
+            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:
@@ -292,6 +343,18 @@ def forward_kinematics(self):
         self.get_logger().debug("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
+        try:
+            self.curr_twist.twist.angular.z = self.curr_twist.twist.linear.x / self.curr_turning_radius
+        except ZeroDivisionError:
+            self.get_logger().warn("Current turning radius was calculated as zero which"
+                                   "is an illegal value. Check your wheel calibration.")
+            self.curr_twist.twist.angular.z = 0.  # turning in place is currently unsupported
+
 
 def main(args=None):
     rclpy.init(args=args)
diff --git a/init_scripts/launch_osr.sh b/init_scripts/launch_osr.sh
@@ -0,0 +1,21 @@
+#!/bin/bash
+# exit on error, and output executed commands to stdout
+set -ex
+
+source osr_paths.sh
+launch_dir=$OSR_CODE_DIR/ROS/osr_bringup/launch
+
+bash -c ". /home/$USER/osr_ws/install/setup.sh"
+bash -c ". /home/$USER/osr_ws/devel/setup.bash"
+bash -c ". /opt/ros/melodic/setup.sh"
+bash -c ". /opt/ros/melodic/setup.bash"
+
+# execute the custom mod launch file if it's available
+if [ -e "$launch_dir/osr_mod.launch" ]; then
+    echo "Launching osr_mod.launch"
+    bash -i -c "roslaunch osr_bringup osr_mod.launch"
+# otherwise go with the default
+else
+    echo "Launching osr.launch"
+    bash -i -c "roslaunch osr_bringup osr.launch"
+fi
diff --git a/init_scripts/osr_paths.sh b/init_scripts/osr_paths.sh
@@ -0,0 +1,3 @@
+## Paths used for OSR code
+# make sure to source (`$ source osr_paths.sh`) this script!
+export OSR_CODE_DIR=$HOME/osr_ws/src/osr-rover-code
diff --git a/init_scripts/osr_startup.service b/init_scripts/osr_startup.service
@@ -6,7 +6,7 @@ After=network.target
 User=ubuntu
 Group=ubuntu
 WorkingDirectory=/home/ubuntu/
-ExecStart=/home/ubuntu/LaunchOSR.sh
+ExecStart=/home/ubuntu/launch_osr.sh
 ExecReload=/bin/kill -HUP $MAINPID
 Restart=always
 
diff --git a/scripts/roboclawtest.py b/scripts/roboclawtest.py
@@ -15,11 +15,17 @@
 if __name__ == "__main__":
     
     address = int(sys.argv[1]) 
-    roboclaw = Roboclaw("/dev/serial1", 115200)
-    try:
-        assert roboclaw.Open() == 1
-    except AssertionError as e:
-        raise e("Could not open comport /dev/serial1, make sure it has the correct permissions and is available")
-
-    print(roboclaw.ReadVersion(address))
-    print(roboclaw.ReadEncM1(address))
+    roboclaw0 = Roboclaw("/dev/serial0", 115200)
+    roboclaw1 = Roboclaw("/dev/serial1", 115200)
+    connected0 = roboclaw0.Open() == 1
+    connected1 = roboclaw1.Open() == 1
+    if connected0:
+        print("Connected to /dev/serial0.")
+        print(roboclaw0.ReadVersion(address))
+        print(roboclaw0.ReadEncM1(address))
+    elif connected1:
+        print("Connected to /dev/serial1.")
+        print(roboclaw1.ReadVersion(address))
+        print(roboclaw1.ReadEncM1(address))
+    else:
+        print("Could not open comport /dev/serial0 or /dev/serial1, make sure it has the correct permissions and is available")
diff --git a/setup/rover_bringup.md b/setup/rover_bringup.md
@@ -21,7 +21,19 @@ they have the appropriate permissions; `sudo chmod a+rw /dev/input/event0`. (The
 Then try again. You might want to consider using udev rules to automate this which you will need for automatic bringup using systemd services below.
 If you need help, please post on GitHub Discussions or on the Slack forum.
 
-## 2 Automatic bringup with init script
+### 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 [systemd script](../init_scripts/launch_osr.sh) will automatically find it.
+
+This is useful, for example, when you don't have the LED screen. In that case you would just remove the `<node name="led_screen" pkg="led_screen" type="arduino_comm.py"/>` line in `osr_mod.launch`.
+
+## 3 Automatic bringup with launch script
 
 Starting scripts on boot using ROS can be a little more difficult than starting scripts on boot normally from
 the Raspberry Pi because of the default permission settings on the RPi and the fact that that ROS cannot
@@ -34,8 +46,9 @@ roslaunch file, and the other creates a system service to start that bash script
 raspberry Pi and execute the following commands.
 ```
 cd ~/osr_ws/src/osr-rover-code/init_scripts
-sudo cp LaunchOSR.sh ~/LaunchOSR.sh
-sudo chmod +x ~/LaunchOSR.sh
+# use symbolic links so we capture updates to these files in the service
+ln -s $(pwd)/launch_osr.sh ~/launch_osr.sh
+ln -s $(pwd)/osr_paths.sh ~/osr_paths.sh
 sudo cp osr_startup.service /etc/systemd/system/osr_startup.service
 sudo chmod 644 /etc/systemd/system/osr_startup.service
 ```
diff --git a/setup/serial_config_info.md b/setup/serial_config_info.md
diff --git a/setup/wheel_odom_example.png b/setup/wheel_odom_example.png

Original file line number	Diff line number	Diff line change
`@@ -0,0 +1,3 @@`
	`1`	`+## Paths used for OSR code`
	`2`	+# make sure to source (`$ source osr_paths.sh`) this script!
	`3`	`+export OSR_CODE_DIR=$HOME/osr_ws/src/osr-rover-code`