From 1cf7830e08fb283c2f3d0b67866468a94f5f2593 Mon Sep 17 00:00:00 2001 From: TheNoobInventor Date: Tue, 2 Apr 2024 01:03:18 +0100 Subject: [PATCH] Update controller manager arguments in launch files --- lidarbot_bringup/config/controllers.yaml | 2 +- .../launch/lidarbot_bringup_launch.py | 177 +++++++++++------- lidarbot_gazebo/launch/gazebo_launch.py | 153 ++++++++------- lidarbot_navigation/CMakeLists.txt | 11 ++ .../lidarbot_navigation/__init__.py | 0 lidarbot_navigation/package.xml | 4 +- .../scripts/trajectory_visualizer.py | 125 +++++++++++++ 7 files changed, 340 insertions(+), 132 deletions(-) create mode 100644 lidarbot_navigation/lidarbot_navigation/__init__.py create mode 100755 lidarbot_navigation/scripts/trajectory_visualizer.py diff --git a/lidarbot_bringup/config/controllers.yaml b/lidarbot_bringup/config/controllers.yaml index 75ae35c..f6b5aa4 100644 --- a/lidarbot_bringup/config/controllers.yaml +++ b/lidarbot_bringup/config/controllers.yaml @@ -44,4 +44,4 @@ imu_broadcaster: # 500 data points used to calculated covariances static_covariance_orientation: [2.63882e-06, 0.0, 0.0, 0.0, 7.50018e-06, 0.0, 0.0, 0.0, 2.89257e-09] static_covariance_angular_velocity: [2.71413e-07, 0.0, 0.0, 0.0, 6.79488e-07, 0.0, 0.0, 0.0, 4.37879e-07] - static_covariance_linear_acceleration: [0.00133755, 0.0, 0.0, 0.0, 0.000209753, 0.0, 0.0, 0.0, 0.00143276] \ No newline at end of file + static_covariance_linear_acceleration: [0.00133755, 0.0, 0.0, 0.0, 0.000209753, 0.0, 0.0, 0.0, 0.00143276] diff --git a/lidarbot_bringup/launch/lidarbot_bringup_launch.py b/lidarbot_bringup/launch/lidarbot_bringup_launch.py index a883cb3..698244b 100644 --- a/lidarbot_bringup/launch/lidarbot_bringup_launch.py +++ b/lidarbot_bringup/launch/lidarbot_bringup_launch.py @@ -1,7 +1,12 @@ import os from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction, RegisterEventHandler +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + TimerAction, + RegisterEventHandler, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, Command @@ -10,129 +15,166 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): # Set the path to different files and folders - pkg_path= FindPackageShare(package='lidarbot_bringup').find('lidarbot_bringup') - pkg_description = FindPackageShare(package='lidarbot_description').find('lidarbot_description') - pkg_teleop= FindPackageShare(package='lidarbot_teleop').find('lidarbot_teleop') - pkg_navigation = FindPackageShare(package='lidarbot_navigation').find('lidarbot_navigation') - - controller_params_file= os.path.join(pkg_path, 'config/controllers.yaml') - twist_mux_params_file = os.path.join(pkg_teleop, 'config/twist_mux.yaml') - ekf_params_file = os.path.join(pkg_navigation, 'config/ekf.yaml') - - # Launch configuration variables - use_sim_time = LaunchConfiguration('use_sim_time') - use_ros2_control = LaunchConfiguration('use_ros2_control') - use_robot_localization = LaunchConfiguration('use_robot_localization') - - # Declare the launch arguments + pkg_path = FindPackageShare(package="lidarbot_bringup").find("lidarbot_bringup") + pkg_description = FindPackageShare(package="lidarbot_description").find( + "lidarbot_description" + ) + pkg_teleop = FindPackageShare(package="lidarbot_teleop").find("lidarbot_teleop") + pkg_navigation = FindPackageShare(package="lidarbot_navigation").find( + "lidarbot_navigation" + ) + + controller_params_file = os.path.join(pkg_path, "config/controllers.yaml") + twist_mux_params_file = os.path.join(pkg_teleop, "config/twist_mux.yaml") + ekf_params_file = os.path.join(pkg_navigation, "config/ekf.yaml") + + # Launch configuration variables + use_sim_time = LaunchConfiguration("use_sim_time") + use_ros2_control = LaunchConfiguration("use_ros2_control") + use_robot_localization = LaunchConfiguration("use_robot_localization") + + # Declare the launch arguments declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='False', - description='Use simulation (Gazebo) clock if true') - + name="use_sim_time", + default_value="False", + description="Use simulation (Gazebo) clock if true", + ) + declare_use_ros2_control_cmd = DeclareLaunchArgument( - name='use_ros2_control', - default_value='True', - description='Use ros2_control if true') + name="use_ros2_control", + default_value="True", + description="Use ros2_control if true", + ) declare_use_robot_localization_cmd = DeclareLaunchArgument( - name='use_robot_localization', - default_value='True', - description='Use robot_localization package if true') + name="use_robot_localization", + default_value="True", + description="Use robot_localization package if true", + ) # Start robot state publisher start_robot_state_publisher_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join(pkg_description, 'launch', 'robot_state_publisher_launch.py')]), - launch_arguments={'use_sim_time': use_sim_time, - 'use_ros2_control': use_ros2_control}.items()) + PythonLaunchDescriptionSource( + [os.path.join(pkg_description, "launch", "robot_state_publisher_launch.py")] + ), + launch_arguments={ + "use_sim_time": use_sim_time, + "use_ros2_control": use_ros2_control, + }.items(), + ) + + robot_description = Command( + ["ros2 param get --hide-type /robot_state_publisher robot_description"] + ) - robot_description = Command(['ros2 param get --hide-type /robot_state_publisher robot_description']) - # Launch controller manager start_controller_manager_cmd = Node( - package='controller_manager', - executable='ros2_control_node', - parameters=[{'robot_description': robot_description}, - controller_params_file]) + package="controller_manager", + executable="ros2_control_node", + parameters=[{"robot_description": robot_description}, controller_params_file], + ) - # Delayed controller manager action - start_delayed_controller_manager = TimerAction(period=2.0, actions=[start_controller_manager_cmd]) + # Delayed controller manager action + start_delayed_controller_manager = TimerAction( + period=2.0, actions=[start_controller_manager_cmd] + ) # Spawn diff_controller start_diff_controller_cmd = Node( - package='controller_manager', - executable='spawner', - arguments=['diff_controller']) + package="controller_manager", + executable="spawner", + arguments=["diff_controller", "--controller-manager", "/controller_manager"], + ) # Delayed diff_drive_spawner action start_delayed_diff_drive_spawner = RegisterEventHandler( event_handler=OnProcessStart( target_action=start_controller_manager_cmd, - on_start=[start_diff_controller_cmd])) + on_start=[start_diff_controller_cmd], + ) + ) # Spawn joint_state_broadcaser start_joint_broadcaster_cmd = Node( # condition=IfCondition(use_ros2_control), - package='controller_manager', - executable='spawner', - arguments=['joint_broadcaster']) + package="controller_manager", + executable="spawner", + arguments=["joint_broadcaster", "--controller-manager", "/controller_manager"], + ) # Delayed joint_broadcaster_spawner action start_delayed_joint_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessStart( target_action=start_controller_manager_cmd, - on_start=[start_joint_broadcaster_cmd])) + on_start=[start_joint_broadcaster_cmd], + ) + ) # Spawn imu_sensor_broadcaser start_imu_broadcaster_cmd = Node( # condition=IfCondition(use_ros2_control), - package='controller_manager', - executable='spawner', - arguments=['imu_broadcaster']) + package="controller_manager", + executable="spawner", + arguments=["imu_broadcaster"], + ) # Delayed imu_broadcaster_spawner action start_delayed_imu_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessStart( target_action=start_controller_manager_cmd, - on_start=[start_imu_broadcaster_cmd])) + on_start=[start_imu_broadcaster_cmd], + ) + ) # Start robot localization using an Extended Kalman Filter start_robot_localization_cmd = Node( condition=IfCondition(use_robot_localization), - package='robot_localization', - executable='ekf_node', - parameters=[ekf_params_file]) - + package="robot_localization", + executable="ekf_node", + parameters=[ekf_params_file], + ) + # Start joystick node - start_joystick_cmd= IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join(pkg_teleop, 'launch', 'joystick_launch.py')])) + start_joystick_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(pkg_teleop, "launch", "joystick_launch.py")] + ) + ) # Start rplidar node - start_rplidar_cmd= IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join(pkg_path, 'launch', 'rplidar_launch.py')])) + start_rplidar_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(pkg_path, "launch", "rplidar_launch.py")] + ) + ) # Start camera node start_camera_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join(pkg_path, 'launch', 'camera_launch.py')])) - - # Start twist mux + PythonLaunchDescriptionSource( + [os.path.join(pkg_path, "launch", "camera_launch.py")] + ) + ) + + # Start twist mux start_twist_mux_cmd = Node( - package='twist_mux', - executable='twist_mux', + package="twist_mux", + executable="twist_mux", parameters=[twist_mux_params_file], - remappings=[('/cmd_vel_out', '/diff_controller/cmd_vel_unstamped')]) + remappings=[("/cmd_vel_out", "/diff_controller/cmd_vel_unstamped")], + ) # Create the launch description and populate ld = LaunchDescription() - + # Declare the launch options ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_use_ros2_control_cmd) ld.add_action(declare_use_robot_localization_cmd) - + # Add any actions ld.add_action(start_robot_state_publisher_cmd) ld.add_action(start_delayed_controller_manager) @@ -144,7 +186,8 @@ def generate_launch_description(): ld.add_action(start_rplidar_cmd) ld.add_action(start_camera_cmd) ld.add_action(start_twist_mux_cmd) - + return ld - # TODO: Launch file summary \ No newline at end of file + # TODO: Launch file summary + diff --git a/lidarbot_gazebo/launch/gazebo_launch.py b/lidarbot_gazebo/launch/gazebo_launch.py index a8f80e8..8252f64 100644 --- a/lidarbot_gazebo/launch/gazebo_launch.py +++ b/lidarbot_gazebo/launch/gazebo_launch.py @@ -1,6 +1,6 @@ -# Launches the lidarbot in Gazebo to be controlled using a joystick. There are a number of launch arguments that can be toggled. -# Such as using the gazebo_ros plugin or the ros2_control plugin, using Gazebo's sim time or not. -# +# Launches the lidarbot in Gazebo to be controlled using a joystick. There are a number of launch arguments that can be toggled. +# Such as using the gazebo_ros plugin or the ros2_control plugin, using Gazebo's sim time or not. +# # File adapted from https://automaticaddison.com import os @@ -14,102 +14,129 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): # Set the path to different files and folders - pkg_path= FindPackageShare(package='lidarbot_gazebo').find('lidarbot_gazebo') - pkg_description = FindPackageShare(package='lidarbot_description').find('lidarbot_description') - pkg_teleop= FindPackageShare(package='lidarbot_teleop').find('lidarbot_teleop') - pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') - pkg_navigation = FindPackageShare(package='lidarbot_navigation').find('lidarbot_navigation') - - gazebo_params_file = os.path.join(pkg_path, 'config/gazebo_params.yaml') - twist_mux_params_file = os.path.join(pkg_teleop, 'config/twist_mux.yaml') - ekf_params_file = os.path.join(pkg_navigation, 'config/ekf.yaml') - world_filename = 'obstacles.world' - world_path = os.path.join(pkg_path, 'worlds', world_filename) + pkg_path = FindPackageShare(package="lidarbot_gazebo").find("lidarbot_gazebo") + pkg_description = FindPackageShare(package="lidarbot_description").find( + "lidarbot_description" + ) + pkg_teleop = FindPackageShare(package="lidarbot_teleop").find("lidarbot_teleop") + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + pkg_navigation = FindPackageShare(package="lidarbot_navigation").find( + "lidarbot_navigation" + ) + + gazebo_params_file = os.path.join(pkg_path, "config/gazebo_params.yaml") + twist_mux_params_file = os.path.join(pkg_teleop, "config/twist_mux.yaml") + ekf_params_file = os.path.join(pkg_navigation, "config/ekf.yaml") + world_filename = "obstacles.world" + world_path = os.path.join(pkg_path, "worlds", world_filename) # Launch configuration variables specific to simulation - use_sim_time = LaunchConfiguration('use_sim_time') - use_ros2_control = LaunchConfiguration('use_ros2_control') - world = LaunchConfiguration('world') - use_robot_localization = LaunchConfiguration('use_robot_localization') + use_sim_time = LaunchConfiguration("use_sim_time") + use_ros2_control = LaunchConfiguration("use_ros2_control") + world = LaunchConfiguration("world") + use_robot_localization = LaunchConfiguration("use_robot_localization") # Declare the launch arguments declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='True', - description='Use simulation (Gazebo) clock if true') - + name="use_sim_time", + default_value="True", + description="Use simulation (Gazebo) clock if true", + ) + declare_use_ros2_control_cmd = DeclareLaunchArgument( - name='use_ros2_control', - default_value='True', - description='Use ros2_control if true') - + name="use_ros2_control", + default_value="True", + description="Use ros2_control if true", + ) + declare_world_cmd = DeclareLaunchArgument( - name='world', + name="world", default_value=world_path, - description='Full path to the world model to load') + description="Full path to the world model to load", + ) declare_use_robot_localization_cmd = DeclareLaunchArgument( - name='use_robot_localization', - default_value='True', - description='Use robot_localization package if true') + name="use_robot_localization", + default_value="True", + description="Use robot_localization package if true", + ) # Start robot state publisher start_robot_state_publisher_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join(pkg_description, 'launch', 'robot_state_publisher_launch.py')]), - launch_arguments={'use_sim_time': use_sim_time, - 'use_ros2_control': use_ros2_control}.items()) + PythonLaunchDescriptionSource( + [os.path.join(pkg_description, "launch", "robot_state_publisher_launch.py")] + ), + launch_arguments={ + "use_sim_time": use_sim_time, + "use_ros2_control": use_ros2_control, + }.items(), + ) - # Launch Gazebo + # Launch Gazebo start_gazebo_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py')]), - launch_arguments={'world': world, 'extra_gazebo_args': '--ros-args --params-file ' + gazebo_params_file}.items()) + PythonLaunchDescriptionSource( + [os.path.join(pkg_gazebo_ros, "launch", "gazebo.launch.py")] + ), + launch_arguments={ + "world": world, + "extra_gazebo_args": "--ros-args --params-file " + gazebo_params_file, + }.items(), + ) # Spawn robot in Gazebo start_spawner_cmd = Node( - package='gazebo_ros', - executable='spawn_entity.py', - output='screen', - arguments=['-topic', 'robot_description', - '-entity', 'lidarbot']) - + package="gazebo_ros", + executable="spawn_entity.py", + output="screen", + arguments=["-topic", "robot_description", "-entity", "lidarbot"], + ) + # Spawn diff_controller start_diff_controller_cmd = Node( condition=IfCondition(use_ros2_control), - package='controller_manager', - executable='spawner', - arguments=['diff_controller']) - + package="controller_manager", + executable="spawner", + arguments=["diff_controller", "--controller-manager", "/controller_manager"], + ) + # Spawn joint_state_broadcaser start_joint_broadcaster_cmd = Node( condition=IfCondition(use_ros2_control), - package='controller_manager', - executable='spawner', - arguments=['joint_broadcaster']) - + package="controller_manager", + executable="spawner", + arguments=["joint_broadcaster", "--controller-manager", "/controller_manager"], + ) + # Start robot localization using an Extended Kalman Filter start_robot_localization_cmd = Node( condition=IfCondition(use_robot_localization), - package='robot_localization', - executable='ekf_node', - parameters=[ekf_params_file]) - + package="robot_localization", + executable="ekf_node", + parameters=[ekf_params_file], + ) + # Start joystick node for use with ros2_control - start_joystick_cmd= IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join(pkg_teleop, 'launch', 'joystick_launch.py')])) + start_joystick_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(pkg_teleop, "launch", "joystick_launch.py")] + ) + ) - # Start twist mux + # Start twist mux start_twist_mux_cmd = Node( - package='twist_mux', - executable='twist_mux', - parameters=[twist_mux_params_file, {'use_sim_time': True}], - remappings=[('/cmd_vel_out', '/diff_controller/cmd_vel_unstamped')]) + package="twist_mux", + executable="twist_mux", + parameters=[twist_mux_params_file, {"use_sim_time": True}], + remappings=[("/cmd_vel_out", "/diff_controller/cmd_vel_unstamped")], + ) # Create the launch description and populate ld = LaunchDescription() - + # Declare the launch options ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_use_ros2_control_cmd) diff --git a/lidarbot_navigation/CMakeLists.txt b/lidarbot_navigation/CMakeLists.txt index aae0033..1f26719 100644 --- a/lidarbot_navigation/CMakeLists.txt +++ b/lidarbot_navigation/CMakeLists.txt @@ -2,10 +2,21 @@ cmake_minimum_required(VERSION 3.8) project(lidarbot_navigation) find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) install( DIRECTORY config launch maps DESTINATION share/${PROJECT_NAME} ) +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + scripts/trajectory_visualizer.py + DESTINATION lib/${PROJECT_NAME} +) + ament_package() diff --git a/lidarbot_navigation/lidarbot_navigation/__init__.py b/lidarbot_navigation/lidarbot_navigation/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/lidarbot_navigation/package.xml b/lidarbot_navigation/package.xml index 57a97e7..978a433 100644 --- a/lidarbot_navigation/package.xml +++ b/lidarbot_navigation/package.xml @@ -4,10 +4,11 @@ lidarbot_navigation 0.0.0 Lidarbot navigation package - Chinedu Amadi + Chinedu Amadi BSD 3-Clause ament_cmake + ament_cmake_python ament_lint_auto ament_lint_common @@ -15,6 +16,7 @@ robot_localization navigation2 nav2_bringup + rclpy ament_cmake diff --git a/lidarbot_navigation/scripts/trajectory_visualizer.py b/lidarbot_navigation/scripts/trajectory_visualizer.py new file mode 100755 index 0000000..5a5109f --- /dev/null +++ b/lidarbot_navigation/scripts/trajectory_visualizer.py @@ -0,0 +1,125 @@ +#!/usr/bin/python3 + +# Node to visualize the robot trajectory in Rviz. +# Adapted from https://github.com/RBinsonB/trajectory_visualization + +import rclpy +from rclpy.node import Node +from rclpy.node import Parameter + +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + PoseWithCovariance, + PoseWithCovarianceStamped, +) + + +class TrajectoryVisualizer(Node): + + def __init__(self, name): + super().__init__(name) + self.load_params() + self.trajectory_path_msg = Path() + self.previous_pose_position = Point() + + # Declare parameters + self.declare_parameter("max_poses", rclpy.Parameter.Type.INTEGER) + self.declare_parameter("threshold", rclpy.Parameter.Type.DOUBLE) + self.declare_parameter("frame_id", rclpy.Parameter.Type.STRING) + + # Setup trajectory path publisher + self.trajectory_path_pub = self.create_publisher(Path, "trajectory_path", 10) + + # Setup subscriber to pose + self.pose_sub = self.create_subscription( + Pose, "pose_topic", self.pose_callback, 10 + ) + + # Setup subscriber to pose stamped + self.pose_stamped_sub = self.create_subscription( + PoseStamped, "pose_stamped_topic", self.pose_stamped_callback, 10 + ) + + # Setup subscriber to pose with covariance + self.pose_cov_sub = self.create_subscription( + PoseWithCovariance, "pose_cov_topic", self.pose_cov_callback, 10 + ) + + # Setup subscriber to pose with covariance stamped + self.pose_cov_stamped_sub = self.create_subscription( + PoseWithCovarianceStamped, + "pose_cov_stamped_topic", + self.pose_cov_stamped_callback, + 10, + ) + + # Setup subscriber to odometry + self.odom_sub = self.create_subscription( + Odometry, "odom_topic", self.odom_callback, 10 + ) + + # Load parameters + def load_params(self): + # Load maximum number of poses in actual path + self.max_poses = Parameter("max_poses", Parameter.Type.INTEGER, 1000) + + # Load threshold for adding a ose to actual path + self.threshold = Parameter("threshold", Parameter.Type.DOUBLE, 0.001) + + # Load parent frame id for the trajectory + self.frame_id = Parameter("frame_id", Parameter.Type.STRING, 'map') + + # Pose callback function + def pose_callback(self, pose_msg): + + self.get_logger().debug( + f"Received pose message with position {pose_msg.pose.position}" + ) + # Process message position and add it to path + self.publish_trajectory_path(pose_msg.position) + + def pose_stamped_callback(self, pose_stamped_msg): + self.get_logger().debug( + f"Received pose message with position {pose_stamped_msg.pose.position}" + ) + if pose_stamped_msg.header.frame_id == self.frame_id: + self.publish_trajectory_path(pose_stamped_msg.pose.position) + else: + self.get_logger().error( + "PoseStamped message frame: {0} does not correspond to trajectory frame {1}", + format(pose_stamped_msg.header.frame_id, self.frame_id), + ) + + def pose_cov_callback(self, pose_cov_msg): + self.get_logger().debug( + "Received pose message with position {pose_msg.position}" + ) + + def pose_cov_stamped_callback(self, pose_cov_stamped_msg): + self.get_logger().debug( + "Received pose message with position {pose_msg.position}" + ) + + def odom_callback(self, odom_msg): + self.get_logger().debug( + "Received pose message with position {pose_msg.position}" + ) + + def publish_trajectory_path(self, position): + pass + + +def main(args=None): + rclpy.init(args=args) + + pose_to_path = TrajectoryVisualizer("pose_to_path") + + rclpy.spin() + rclpy.shutdown(pose_to_path) + + +if __name__ == "__main__": + main()