From 55ea7c568aa537ae5dd55b21dd688a8af56c3b9b Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Sun, 26 Nov 2023 16:14:25 -0700 Subject: [PATCH] Get Hardware working --- .../config/urg_node_ethernet.yaml | 18 +++ .../launch/hardware_c300_launch.py | 98 ++++++++++++++++ c300/c300_bringup/launch/urg_node_launch.py | 39 +++++++ .../c300_driver/diff_drive_odom.py | 4 +- c300/c300_driver/c300_driver/roboclaw_3.py | 18 +-- .../c300_driver/c300_driver/twist2roboclaw.py | 81 +++++++++++-- c300/c300_driver/launch/driver.launch.py | 23 ++++ .../launch/navigation.launch.py | 4 +- .../params/nav2_params_mppi.yaml | 13 +-- c300/c300_navigation/rviz/navigation.rviz | 108 +++++++++++++++--- 10 files changed, 355 insertions(+), 51 deletions(-) create mode 100644 c300/c300_bringup/config/urg_node_ethernet.yaml create mode 100644 c300/c300_bringup/launch/hardware_c300_launch.py create mode 100644 c300/c300_bringup/launch/urg_node_launch.py create mode 100644 c300/c300_driver/launch/driver.launch.py diff --git a/c300/c300_bringup/config/urg_node_ethernet.yaml b/c300/c300_bringup/config/urg_node_ethernet.yaml new file mode 100644 index 0000000..58d1897 --- /dev/null +++ b/c300/c300_bringup/config/urg_node_ethernet.yaml @@ -0,0 +1,18 @@ +urg_node: + ros__parameters: + angle_max: 3.14 + angle_min: -3.14 + ip_address: "192.168.0.10" + ip_port: 10940 + serial_baud: 115200 + laser_frame_id: "base_laser" + calibrate_time: false + default_user_latency: 0.0 + diagnostics_tolerance: 0.05 + diagnostics_window_time: 5.0 + error_limit: 4 + get_detailed_status: false + publish_intensity: false + publish_multiecho: false + cluster: 1 + skip: 0 diff --git a/c300/c300_bringup/launch/hardware_c300_launch.py b/c300/c300_bringup/launch/hardware_c300_launch.py new file mode 100644 index 0000000..22f70cc --- /dev/null +++ b/c300/c300_bringup/launch/hardware_c300_launch.py @@ -0,0 +1,98 @@ +# -*- coding: utf-8 -*- +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ( + OpaqueFunction, + DeclareLaunchArgument, + TimerAction, + IncludeLaunchDescription, +) +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import IfCondition, UnlessCondition + + +def launch_setup(context, *args, **kwargs): + # Configure some helper variables and paths + pkg_c300_navigation = get_package_share_directory("c300_navigation") + pkg_driver = get_package_share_directory("c300_driver") + pkg_robot_description = get_package_share_directory("c300_description") + pkg_bringup = get_package_share_directory("c300_bringup") + + # Hardware LIDAR + lidar_launch_py = PythonLaunchDescriptionSource( + [ + pkg_bringup, + "/launch/urg_node_launch.py", + ] + ) + lidar_launch = IncludeLaunchDescription( + lidar_launch_py, + ) + + # Hardware Driver + driver_launch_py = PythonLaunchDescriptionSource( + [ + pkg_driver, + "/launch/driver.launch.py", + ] + ) + driver_launch = IncludeLaunchDescription( + driver_launch_py, + ) + + # Parse xacro and publish robot state + robot_description_path = os.path.join( + pkg_robot_description, "urdf", "c300_base.urdf" + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + robot_description_path, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="c300_robot_state_publisher", + output="both", + parameters=[robot_description, {"publish_frequency", "50"}], + ) + + # Bringup Navigation2 + nav2_launch_py = PythonLaunchDescriptionSource( + [ + pkg_c300_navigation, + "/launch/navigation.launch.py", + ] + ) + nav2_launch = IncludeLaunchDescription( + nav2_launch_py, + ) + + nodes_and_launches = [ + robot_state_publisher_node, + lidar_launch, + driver_launch, + TimerAction(period=5.0, actions=[nav2_launch]), + ] + + return nodes_and_launches + + +def generate_launch_description(): + return LaunchDescription( + [OpaqueFunction(function=launch_setup)] + ) diff --git a/c300/c300_bringup/launch/urg_node_launch.py b/c300/c300_bringup/launch/urg_node_launch.py new file mode 100644 index 0000000..7edb2db --- /dev/null +++ b/c300/c300_bringup/launch/urg_node_launch.py @@ -0,0 +1,39 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + + +def generate_launch_description(): + config_dir = get_package_share_directory('c300_bringup') + launch_description = LaunchDescription([ + DeclareLaunchArgument( + 'sensor_interface', + default_value='ethernet', + description='sensor_interface: supported: serial, ethernet')]) + + def expand_param_file_name(context): + param_file = os.path.join( + config_dir, 'config', + 'urg_node_' + context.launch_configurations['sensor_interface'] + '.yaml') + if os.path.exists(param_file): + return [SetLaunchConfiguration('param', param_file)] + + param_file_path = OpaqueFunction(function=expand_param_file_name) + launch_description.add_action(param_file_path) + + hokuyo_node = Node( + package='urg_node', executable='urg_node_driver', output='screen', + name='urg_node', + parameters=[LaunchConfiguration('param')] + ) + + launch_description.add_action(hokuyo_node) + return launch_description diff --git a/c300/c300_driver/c300_driver/diff_drive_odom.py b/c300/c300_driver/c300_driver/diff_drive_odom.py index 52272e3..7eba239 100644 --- a/c300/c300_driver/c300_driver/diff_drive_odom.py +++ b/c300/c300_driver/c300_driver/diff_drive_odom.py @@ -37,10 +37,10 @@ def step(self, position, velocity): theta = self._radius * (wheel_r - wheel_l) / self._separation self._robot_pose = ( self._robot_pose[0] + delta_s * cos(self._robot_pose[2] + (theta / 2.0)), - self._robot_pose[1] + delta_s * sin(self._robot_pose[2] + (theta / 2.0)), + self._robot_pose[1] - delta_s * sin(self._robot_pose[2] + (theta / 2.0)), self._robot_pose[2] + theta, ) - q = quaternion_from_euler(0.0, 0.0, self._robot_pose[2]) + q = quaternion_from_euler(0.0, 0.0, -self._robot_pose[2]) self._last_time = now diff --git a/c300/c300_driver/c300_driver/roboclaw_3.py b/c300/c300_driver/c300_driver/roboclaw_3.py index bd648f7..5b67966 100644 --- a/c300/c300_driver/c300_driver/roboclaw_3.py +++ b/c300/c300_driver/c300_driver/roboclaw_3.py @@ -14,6 +14,12 @@ def __init__(self, comport, rate, timeout=0.01, retries=3): self.timeout = timeout self._trystimeout = retries self._crc = 0 + self._port = serial.Serial( + port=self.comport, + baudrate=self.rate, + timeout=1, + interCharTimeout=self.timeout, + ) # Command Enums class Cmd: @@ -1195,15 +1201,3 @@ def WriteEeprom(self, address, ee_address, ee_word): if tries == 0: break return False - - def Open(self): - try: - self._port = serial.Serial( - port=self.comport, - baudrate=self.rate, - timeout=1, - interCharTimeout=self.timeout, - ) - except: - return 0 - return 1 diff --git a/c300/c300_driver/c300_driver/twist2roboclaw.py b/c300/c300_driver/c300_driver/twist2roboclaw.py index 3437bd2..9ab64e4 100644 --- a/c300/c300_driver/c300_driver/twist2roboclaw.py +++ b/c300/c300_driver/c300_driver/twist2roboclaw.py @@ -19,18 +19,52 @@ from . import roboclaw_3 from . import diff_drive_odom +from tf2_ros import TransformBroadcaster + +from geometry_msgs.msg import TransformStamped from geometry_msgs.msg import Twist +from sensor_msgs.msg import LaserScan, JointState +from nav_msgs.msg import Odometry + +from std_msgs.msg import Header import math from pprint import pprint +def euler_from_quaternion(odom_msg): + """ + Convert a quaternion into euler angles (roll, pitch, yaw) + roll is rotation around x in radians (counterclockwise) + pitch is rotation around y in radians (counterclockwise) + yaw is rotation around z in radians (counterclockwise) + """ + x = odom_msg.pose.pose.orientation.x + y = odom_msg.pose.pose.orientation.y + z = odom_msg.pose.pose.orientation.z + w = odom_msg.pose.pose.orientation.w + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + roll_x = math.atan2(t0, t1) + + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + pitch_y = math.asin(t2) + + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + yaw_z = math.atan2(t3, t4) + + return roll_x, pitch_y, yaw_z # in radians + + class RoboclawTwistSubscriber(Node): def __init__(self): super().__init__("roboclaw_twist_subscriber") - cmd_vel_topic = "cmd_vel" + cmd_vel_topic = "c3pzero/cmd_vel" self.wheel_radius = 0.1715 # meters self.wheel_circumference = 2 * math.pi * self.wheel_radius # meters self.ppr = 11600 # pulses per wheel revolution @@ -40,11 +74,14 @@ def __init__(self): Twist, cmd_vel_topic, self.twist_listener_callback, 10 ) self.subscription # prevent unused variable warning - - self.rc = roboclaw_3.Roboclaw("/dev/ttyACM0", 115200) - - if not self.rc.Open(): + self.odom_publisher_ = self.create_publisher(Odometry, "c3pzero/odometry", 10) + self.br = TransformBroadcaster(self) + self.joint_publisher_ = self.create_publisher(JointState, "joint_states", 10) + try: + self.rc = roboclaw_3.Roboclaw("/dev/ttyACM0", 115200) + except: self.get_logger().error("failed to open port") + raise self.rc_address = 0x80 version = self.rc.ReadVersion(self.rc_address) @@ -67,10 +104,8 @@ def __init__(self): def twist_listener_callback(self, msg): # self.get_logger().info('X_vel: %f, Z_rot: %f' % (0.4*msg.linear.x, msg.angular.z)) - right_wheel = ( - 0.2 * msg.linear.x + (0.3 * msg.angular.z * self.wheel_track) / 2 - ) # meters / sec - left_wheel = 0.2 * msg.linear.x - (0.3 * msg.angular.z * self.wheel_track) / 2 + right_wheel = msg.linear.x + (msg.angular.z * self.wheel_track) / 2 # meters / sec + left_wheel = msg.linear.x - (msg.angular.z * self.wheel_track) / 2 # meters / sec wheel_cmds = self.mps_to_pps((right_wheel, left_wheel)) self.rc.SpeedM1(self.rc_address, wheel_cmds[0]) @@ -106,14 +141,38 @@ def odom_callback(self): odom_msg = self.diff_drive_odom.step(wheel_pos, wheel_speed) # pprint(odom_msg.pose.pose.position) - self.get_logger().info( + self.get_logger().debug( "Pose: x=%f, y=%f theta=%f" % ( odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, - odom_msg.pose.pose.orientation.z, + euler_from_quaternion(odom_msg)[2], ) ) + self.odom_publisher_.publish(odom_msg) + + t = TransformStamped() + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = "odom" + t.child_frame_id = "base_link" + t.transform.translation.x = odom_msg.pose.pose.position.x + t.transform.translation.y = odom_msg.pose.pose.position.y + t.transform.translation.z = 0.0 + t.transform.rotation.x = odom_msg.pose.pose.orientation.x + t.transform.rotation.y = odom_msg.pose.pose.orientation.y + t.transform.rotation.z = odom_msg.pose.pose.orientation.z + t.transform.rotation.w = odom_msg.pose.pose.orientation.w + + # Send the transformation + self.br.sendTransform(t) + + wheel_state = JointState() + wheel_state.header.stamp = self.get_clock().now().to_msg() + wheel_state.name = ['drivewhl_r_joint', 'drivewhl_l_joint'] + wheel_state.position = wheel_pos + wheel_state.velocity = wheel_speed + wheel_state.effort = [] + self.joint_publisher_.publish(wheel_state) def mps_to_pps(self, wheel_speed): right_wheel_pluses = int(wheel_speed[0] / self.wheel_circumference * self.ppr) diff --git a/c300/c300_driver/launch/driver.launch.py b/c300/c300_driver/launch/driver.launch.py new file mode 100644 index 0000000..af3b8a4 --- /dev/null +++ b/c300/c300_driver/launch/driver.launch.py @@ -0,0 +1,23 @@ +# -*- coding: utf-8 -*- +import os + +from ament_index_python.packages import get_package_share_directory + +import launch +import launch_ros.actions + + +def generate_launch_description(): + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="c300_driver", + executable="twist2roboclaw", + name="driver", + remappings={ + ("c3pzero/cmd_vel", "/cmd_vel"), + ("/c3pzero/odometry", "/odom") + }, + ), + ] + ) diff --git a/c300/c300_navigation/launch/navigation.launch.py b/c300/c300_navigation/launch/navigation.launch.py index eb304b3..4d441e4 100644 --- a/c300/c300_navigation/launch/navigation.launch.py +++ b/c300/c300_navigation/launch/navigation.launch.py @@ -64,7 +64,7 @@ def generate_launch_description(): ) declare_slam_cmd = DeclareLaunchArgument( - "slam", default_value="False", description="Whether run a SLAM" + "slam", default_value="True", description="Whether run a SLAM" ) declare_map_yaml_cmd = DeclareLaunchArgument( @@ -75,7 +75,7 @@ def generate_launch_description(): declare_use_sim_time_cmd = DeclareLaunchArgument( "use_sim_time", - default_value="True", + default_value="False", description="Use simulation (Gazebo) clock if True", ) diff --git a/c300/c300_navigation/params/nav2_params_mppi.yaml b/c300/c300_navigation/params/nav2_params_mppi.yaml index 9250103..0869b8f 100644 --- a/c300/c300_navigation/params/nav2_params_mppi.yaml +++ b/c300/c300_navigation/params/nav2_params_mppi.yaml @@ -1,6 +1,5 @@ amcl: ros__parameters: - use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -46,7 +45,6 @@ amcl: bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom @@ -104,7 +102,6 @@ bt_navigator: controller_server: ros__parameters: - use_sim_time: True controller_frequency: 30.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 @@ -226,7 +223,7 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: True + rolling_window: True width: 10 height: 10 @@ -280,7 +277,7 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True + width: 20 height: 20 rolling_window: True @@ -313,12 +310,10 @@ global_costmap: map_server: ros__parameters: - use_sim_time: True yaml_filename: "house.yaml" # Change what map to use map_saver: ros__parameters: - use_sim_time: True save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -327,7 +322,6 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -337,7 +331,6 @@ planner_server: smoother_server: ros__parameters: - use_sim_time: True smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" @@ -362,7 +355,6 @@ behavior_server: global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: True simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 @@ -370,7 +362,6 @@ behavior_server: robot_state_publisher: ros__parameters: - use_sim_time: True waypoint_follower: ros__parameters: diff --git a/c300/c300_navigation/rviz/navigation.rviz b/c300/c300_navigation/rviz/navigation.rviz index ecea023..f1e57a7 100644 --- a/c300/c300_navigation/rviz/navigation.rviz +++ b/c300/c300_navigation/rviz/navigation.rviz @@ -8,7 +8,7 @@ Panels: - /TF1/Frames1 - /TF1/Tree1 Splitter Ratio: 0.5833333134651184 - Tree Height: 344 + Tree Height: 333 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -62,6 +62,49 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_laser: + Alpha: 1 + Show Axes: false + Show Trail: false + base_laser_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + drivewhl_l_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + drivewhl_r_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lr_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + platform_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rr_caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Mass Properties: Inertia: false Mass: false @@ -75,13 +118,52 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false + base_footprint: + Value: true + base_laser: + Value: true + base_laser_mount: + Value: true + base_link: + Value: true + drivewhl_l_link: + Value: true + drivewhl_r_link: + Value: true + lr_caster: + Value: true + map: + Value: true + odom: + Value: true + platform_link: + Value: true + rr_caster: + Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: - {} + map: + odom: + base_link: + base_footprint: + {} + base_laser_mount: + base_laser: + {} + drivewhl_l_link: + {} + drivewhl_r_link: + {} + lr_caster: + {} + platform_link: + {} + rr_caster: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -93,8 +175,8 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity + Color: 255; 0; 0 + Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false @@ -523,7 +605,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -1.575000286102295 + Angle: -1.4350003004074097 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -533,21 +615,21 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 81.79286193847656 + Scale: 71.97771453857422 Target Frame: Value: TopDownOrtho (rviz_default_plugins) - X: 0.18391530215740204 - Y: -0.3650076985359192 + X: 0.13642188906669617 + Y: 3.791999101638794 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1043 + Height: 1016 Hide Left Dock: false Hide Right Dock: true Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000282000003bdfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000193000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000001d4000002240000012300fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004f8000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000282000003a2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000188000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000001c9000002140000012300fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004b2000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RealsenseCamera: collapsed: false Selection: @@ -556,6 +638,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 0 - Y: 0 + Width: 1850 + X: 70 + Y: 27