From f9354dc77a6de4ed1473ae49f501e0d99cc4964e Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 8 Mar 2023 17:54:27 -0500 Subject: [PATCH 01/14] Working multi robot sim --- .../config/hazard_vector_params.yaml | 2 +- .../config/ir_intensity_vector_params.yaml | 2 +- .../config/kidnap_estimator_params.yaml | 4 +- .../config/mock_params.yaml | 2 +- .../config/robot_state_params.yaml | 8 +- .../config/ui_mgr_params.yaml | 6 +- .../config/wheel_status_params.yaml | 4 +- .../launch/create3_nodes.launch.py | 7 +- .../launch/dock_description.launch.py | 9 +- .../launch/robot_description.launch.py | 29 +++- .../irobot_create_control/config/control.yaml | 127 +++++++------- .../launch/include/control.py | 9 +- .../urdf/create3.urdf.xacro | 14 +- .../src/hazards_vector_publisher.cpp | 2 +- .../src/ir_intensity_vector_publisher.cpp | 2 +- .../src/kidnap_estimator_publisher.cpp | 4 +- .../src/mock_publisher.cpp | 2 +- .../irobot_create_nodes/src/robot_state.cpp | 8 +- .../irobot_create_nodes/src/ui_mgr.cpp | 6 +- .../src/wheels_publisher.cpp | 4 +- .../config/pose_republisher_params.yaml | 14 +- .../launch/create3_ignition.launch.py | 84 ++-------- .../launch/create3_ignition_nodes.launch.py | 19 ++- .../create3_ros_ignition_bridge.launch.py | 49 ++---- .../launch/create3_spawn_robot.launch.py | 156 ++++++++++++++++++ .../interface_buttons_node.cpp | 2 +- .../pose_republisher_node.cpp | 14 +- .../src/sensors/ir_opcode.cpp | 4 +- .../src/sensors/mouse.cpp | 2 +- 29 files changed, 370 insertions(+), 225 deletions(-) create mode 100644 irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn_robot.launch.py diff --git a/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml index 6e4d5691..2198ad0f 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml @@ -2,7 +2,7 @@ hazards_vector_publisher: ros__parameters: # Hazard detection publisher topic - publisher_topic: /hazard_detection + publisher_topic: hazard_detection publish_rate: 62.0 subscription_topics: # Bumper topic diff --git a/irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml index 63f25408..b931647d 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml @@ -2,7 +2,7 @@ ir_intensity_vector_publisher: ros__parameters: # IR intensity publisher topic - publisher_topic: /ir_intensity + publisher_topic: ir_intensity publish_rate: 62.0 subscription_topics: # IR intensity topics diff --git a/irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml index 37906ab4..9e8d3caf 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml @@ -2,6 +2,6 @@ kidnap_estimator_publisher: ros__parameters: # Kidnap status publisher topic - kidnap_status_topic: /kidnap_status + kidnap_status_topic: kidnap_status # Subscription topics - hazard_topic: /hazard_detection + hazard_topic: hazard_detection diff --git a/irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml index 3a300bb7..8cea8f91 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml @@ -2,6 +2,6 @@ mock_publisher: ros__parameters: # Mock slip status publisher topic - slip_status_topic: /slip_status + slip_status_topic: slip_status # Publishers rate slip_status_publish_rate: 62.0 diff --git a/irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml index b89d0226..a182c38f 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml @@ -2,16 +2,16 @@ robot_state: ros__parameters: # Stop status publisher topic - stop_status_topic: /stop_status + stop_status_topic: stop_status # Mock battery state publisher topic - battery_state_topic: /battery_state + battery_state_topic: battery_state # Publishers rate kidnap_status_publish_rate: 1.0 stop_status_publish_rate: 62.0 battery_state_publish_rate: 0.1 # Subscription topics - dock_topic: /dock_status - wheel_vels_topic: /odom + dock_topic: dock_status + wheel_vels_topic: odom # Stop status position difference tolerance linear_velocity_tolerance: 0.01 angular_velocity_tolerance: 0.1 diff --git a/irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml index 34a07346..3298d059 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml @@ -2,9 +2,9 @@ ui_mgr: ros__parameters: # Buttons publisher topic - button_topic: /interface_buttons + button_topic: interface_buttons # Publishers rate buttons_publish_rate: 1.0 # Subscription topics - lightring_topic: /cmd_lightring - audio_topic: /cmd_audio + lightring_topic: cmd_lightring + audio_topic: cmd_audio diff --git a/irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml index 92e5e3a7..168e828b 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml @@ -8,6 +8,6 @@ wheel_status_publisher: # Wheel radius wheel_radius: 0.03575 # Wheels angular velocity topic - velocity_topic: /wheel_vels + velocity_topic: wheel_vels # Wheels' net encoder ticks topic - ticks_topic: /wheel_ticks + ticks_topic: wheel_ticks diff --git a/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py index 50c3cee8..4fff821f 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py @@ -44,7 +44,8 @@ def generate_launch_description(): # Includes diffdrive_controller = IncludeLaunchDescription( - PythonLaunchDescriptionSource([control_launch_file]) + PythonLaunchDescriptionSource([control_launch_file]), + launch_arguments=[('robot_name', LaunchConfiguration('robot_name'))] ) # Publish hazards vector @@ -74,6 +75,10 @@ def generate_launch_description(): executable='motion_control', parameters=[{'use_sim_time': True}], output='screen', + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ] ) # Publish wheel status diff --git a/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py index 1df2116b..4d0fc371 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py @@ -37,6 +37,7 @@ def generate_launch_description(): visualize_rays = LaunchConfiguration('visualize_rays') gazebo_simulator = LaunchConfiguration('gazebo') + namespace = LaunchConfiguration('namespace', default='') state_publisher = Node( package='robot_state_publisher', @@ -53,6 +54,8 @@ def generate_launch_description(): ], remappings=[ ('robot_description', 'standard_dock_description'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static') ], ) @@ -64,7 +67,11 @@ def generate_launch_description(): # According to documentation (http://wiki.ros.org/tf2_ros): # the order is yaw, pitch, roll yaw, '0', '0', - 'odom', 'std_dock_link'], + [namespace, '/odom'], [namespace, '/std_dock_link']], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ], output='screen', ) diff --git a/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py index 3d21aba2..3009ce52 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py @@ -10,21 +10,36 @@ from launch.substitutions.launch_configuration import LaunchConfiguration from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + ARGUMENTS = [ DeclareLaunchArgument('gazebo', default_value='classic', choices=['classic', 'ignition'], description='Which gazebo simulator to use'), DeclareLaunchArgument('visualize_rays', default_value='false', choices=['true', 'false'], - description='Enable/disable ray visualization') + description='Enable/disable ray visualization'), + DeclareLaunchArgument('robot_name', default_value='create3', + description='Robot name'), ] def generate_launch_description(): pkg_create3_description = get_package_share_directory('irobot_create_description') + pkg_create3_control = get_package_share_directory('irobot_create_control') xacro_file = PathJoinSubstitution([pkg_create3_description, 'urdf', 'create3.urdf.xacro']) gazebo_simulator = LaunchConfiguration('gazebo') visualize_rays = LaunchConfiguration('visualize_rays') + namespace = LaunchConfiguration('robot_name') + + control_params_file = PathJoinSubstitution( + [pkg_create3_control, 'config', 'control.yaml']) + + namespaced_control_params_file = RewrittenYaml( + source_file=control_params_file, + root_key=namespace, + param_rewrites={}, + ) robot_state_publisher = Node( package='robot_state_publisher', @@ -37,8 +52,14 @@ def generate_launch_description(): Command( ['xacro', ' ', xacro_file, ' ', 'gazebo:=', gazebo_simulator, ' ', - 'visualize_rays:=', visualize_rays])}, + 'visualize_rays:=', visualize_rays, ' ', + 'namespace:=', namespace, ' ', + 'control_params:=', control_params_file])}, ], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ] ) joint_state_publisher = Node( @@ -47,6 +68,10 @@ def generate_launch_description(): name='joint_state_publisher', output='screen', parameters=[{'use_sim_time': True}], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ] ) # Define LaunchDescription variable diff --git a/irobot_create_common/irobot_create_control/config/control.yaml b/irobot_create_common/irobot_create_control/config/control.yaml index 31e64f5f..05db3cc9 100644 --- a/irobot_create_common/irobot_create_control/config/control.yaml +++ b/irobot_create_common/irobot_create_control/config/control.yaml @@ -1,77 +1,78 @@ -controller_manager: - ros__parameters: - update_rate: 1000 # Hz +/**: + controller_manager: + ros__parameters: + update_rate: 1000 # Hz - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster - diffdrive_controller: - type: diff_drive_controller/DiffDriveController + diffdrive_controller: + type: diff_drive_controller/DiffDriveController -diffdrive_controller: - ros__parameters: - use_sim_time: True - left_wheel_names: ["left_wheel_joint"] - right_wheel_names: ["right_wheel_joint"] + diffdrive_controller: + ros__parameters: + use_sim_time: True + left_wheel_names: ["left_wheel_joint"] + right_wheel_names: ["right_wheel_joint"] - wheel_separation: 0.233 - wheel_radius: 0.03575 + wheel_separation: 0.233 + wheel_radius: 0.03575 - # Multiplier applied to the wheel separation parameter. - # This is used to account for a difference between the robot model and a real robot. - wheel_separation_multiplier: 1.0 + # Multiplier applied to the wheel separation parameter. + # This is used to account for a difference between the robot model and a real robot. + wheel_separation_multiplier: 1.0 - # Multipliers applied to the wheel radius parameter. - # This is used to account for a difference between the robot model and a real robot. - left_wheel_radius_multiplier: 1.0 - right_wheel_radius_multiplier: 1.0 + # Multipliers applied to the wheel radius parameter. + # This is used to account for a difference between the robot model and a real robot. + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 - publish_rate: 62.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + publish_rate: 62.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - open_loop: false - enable_odom_tf: true + open_loop: false + enable_odom_tf: true - cmd_vel_timeout: 0.5 - use_stamped_vel: false + cmd_vel_timeout: 0.5 + use_stamped_vel: false - # Preserve turning radius when limiting speed/acceleration/jerk - preserve_turning_radius: true + # Preserve turning radius when limiting speed/acceleration/jerk + preserve_turning_radius: true - # Publish limited velocity - publish_cmd: true + # Publish limited velocity + publish_cmd: true - # Publish wheel data - publish_wheel_data: true + # Publish wheel data + publish_wheel_data: true - # Velocity and acceleration limits in cartesian - # These limits do not factor in per wheel limits - # that might be exceeded when linear and angular are combined - # Whenever a min_* is unspecified, default to -max_* - linear.x.has_velocity_limits: true - linear.x.has_acceleration_limits: true - linear.x.has_jerk_limits: false - # This is max if system is safety_override "full" - # motion_control_node will bound this to 0.306 if safety enabled (as is by default) - linear.x.max_velocity: 0.46 - linear.x.min_velocity: -0.46 - linear.x.max_acceleration: 0.9 - # Not using jerk limits yet - # linear.x.max_jerk: 0.0 - # linear.x.min_jerk: 0.0 + # Velocity and acceleration limits in cartesian + # These limits do not factor in per wheel limits + # that might be exceeded when linear and angular are combined + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + # This is max if system is safety_override "full" + # motion_control_node will bound this to 0.306 if safety enabled (as is by default) + linear.x.max_velocity: 0.46 + linear.x.min_velocity: -0.46 + linear.x.max_acceleration: 0.9 + # Not using jerk limits yet + # linear.x.max_jerk: 0.0 + # linear.x.min_jerk: 0.0 - angular.z.has_velocity_limits: true - angular.z.has_acceleration_limits: true - angular.z.has_jerk_limits: false - angular.z.max_velocity: 1.9 - angular.z.min_velocity: -1.9 - # Using 0.9 linear for each wheel, assuming one wheel accel to .9 - # and other to -.9 with wheelbase leads to 7.725 rad/s^2 - angular.z.max_acceleration: 7.725 - angular.z.min_acceleration: -7.725 - # Not using jerk limits yet - # angular.z.max_jerk: 0.0 - # angular.z.min_jerk: 0.0 + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 1.9 + angular.z.min_velocity: -1.9 + # Using 0.9 linear for each wheel, assuming one wheel accel to .9 + # and other to -.9 with wheelbase leads to 7.725 rad/s^2 + angular.z.max_acceleration: 7.725 + angular.z.min_acceleration: -7.725 + # Not using jerk limits yet + # angular.z.max_jerk: 0.0 + # angular.z.min_jerk: 0.0 diff --git a/irobot_create_common/irobot_create_control/launch/include/control.py b/irobot_create_common/irobot_create_control/launch/include/control.py index f8a781d9..f1389ee0 100644 --- a/irobot_create_common/irobot_create_control/launch/include/control.py +++ b/irobot_create_common/irobot_create_control/launch/include/control.py @@ -7,28 +7,31 @@ from launch import LaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node def generate_launch_description(): pkg_create3_control = get_package_share_directory('irobot_create_control') + robot_name = LaunchConfiguration('robot_name') + control_params_file = PathJoinSubstitution( [pkg_create3_control, 'config', 'control.yaml']) diffdrive_controller_node = Node( package='controller_manager', executable='spawner', + namespace=robot_name, # Namespace is not pushed when used in EventHandler parameters=[control_params_file], - arguments=['diffdrive_controller', '-c', '/controller_manager'], + arguments=['diffdrive_controller', '-c', 'controller_manager'], output='screen', ) joint_state_broadcaster_spawner = Node( package='controller_manager', executable='spawner', - arguments=['joint_state_broadcaster', '-c', '/controller_manager'], + arguments=['joint_state_broadcaster', '-c', 'controller_manager'], output='screen', ) diff --git a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro index d351f11b..cec2b66e 100644 --- a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro @@ -14,6 +14,12 @@ + + + + + + @@ -119,6 +125,7 @@ $(find irobot_create_control)/config/control.yaml ~/odom:=odom + $(arg namespace) @@ -127,9 +134,13 @@ - $(find irobot_create_control)/config/control.yaml + $(find irobot_create_control)/config/control.yaml ~/odom:=odom + /tf:=tf + /tf_static:=tf_static + /diagnostics:=diagnostics + $(arg namespace) @@ -289,5 +300,4 @@ - diff --git a/irobot_create_common/irobot_create_nodes/src/hazards_vector_publisher.cpp b/irobot_create_common/irobot_create_nodes/src/hazards_vector_publisher.cpp index d0028071..6504ad31 100644 --- a/irobot_create_common/irobot_create_nodes/src/hazards_vector_publisher.cpp +++ b/irobot_create_common/irobot_create_nodes/src/hazards_vector_publisher.cpp @@ -16,7 +16,7 @@ HazardsVectorPublisher::HazardsVectorPublisher(const rclcpp::NodeOptions & optio { // Topic parameter to publish hazards vector to publisher_topic_ = - this->declare_parameter("publisher_topic", "/hazard_detection"); + this->declare_parameter("publisher_topic", "hazard_detection"); // Subscription topics parameter subscription_topics_ = diff --git a/irobot_create_common/irobot_create_nodes/src/ir_intensity_vector_publisher.cpp b/irobot_create_common/irobot_create_nodes/src/ir_intensity_vector_publisher.cpp index 93d10d35..e3888ac7 100644 --- a/irobot_create_common/irobot_create_nodes/src/ir_intensity_vector_publisher.cpp +++ b/irobot_create_common/irobot_create_nodes/src/ir_intensity_vector_publisher.cpp @@ -16,7 +16,7 @@ IrIntensityVectorPublisher::IrIntensityVectorPublisher(const rclcpp::NodeOptions { // Topic parameter to publish IR intensity vector to publisher_topic_ = - this->declare_parameter("publisher_topic", "/ir_intensity"); + this->declare_parameter("publisher_topic", "ir_intensity"); // Subscription topics parameter subscription_topics_ = diff --git a/irobot_create_common/irobot_create_nodes/src/kidnap_estimator_publisher.cpp b/irobot_create_common/irobot_create_nodes/src/kidnap_estimator_publisher.cpp index c1d71680..9f1deab1 100644 --- a/irobot_create_common/irobot_create_nodes/src/kidnap_estimator_publisher.cpp +++ b/irobot_create_common/irobot_create_nodes/src/kidnap_estimator_publisher.cpp @@ -16,11 +16,11 @@ KidnapEstimator::KidnapEstimator(const rclcpp::NodeOptions & options) { // Topic parameter to publish kidnap status to kidnap_status_publisher_topic_ = - this->declare_parameter("kidnap_status_topic", "/kidnap_status"); + this->declare_parameter("kidnap_status_topic", "kidnap_status"); // Subscriber topics hazard_subscription_topic_ = - this->declare_parameter("hazard_topic", "/hazard_detection"); + this->declare_parameter("hazard_topic", "hazard_detection"); // Define kidnap status publisher kidnap_status_publisher_ = create_publisher( diff --git a/irobot_create_common/irobot_create_nodes/src/mock_publisher.cpp b/irobot_create_common/irobot_create_nodes/src/mock_publisher.cpp index e99237e0..938db1f6 100644 --- a/irobot_create_common/irobot_create_nodes/src/mock_publisher.cpp +++ b/irobot_create_common/irobot_create_nodes/src/mock_publisher.cpp @@ -19,7 +19,7 @@ MockPublisher::MockPublisher(const rclcpp::NodeOptions & options) { // Topic parameter to publish slip status to slip_status_publisher_topic_ = - this->declare_parameter("slip_status_topic", "/slip_status"); + this->declare_parameter("slip_status_topic", "slip_status"); // Publish rate parameters in Hz const double slip_status_publish_rate = diff --git a/irobot_create_common/irobot_create_nodes/src/robot_state.cpp b/irobot_create_common/irobot_create_nodes/src/robot_state.cpp index 65347b03..ca1c96bd 100644 --- a/irobot_create_common/irobot_create_nodes/src/robot_state.cpp +++ b/irobot_create_common/irobot_create_nodes/src/robot_state.cpp @@ -22,16 +22,16 @@ RobotState::RobotState(const rclcpp::NodeOptions & options) // Topic parameter to publish battery state to battery_state_publisher_topic_ = - this->declare_parameter("battery_state_topic", "/battery_state"); + this->declare_parameter("battery_state_topic", "battery_state"); // Topic parameter to publish stop status to stop_status_publisher_topic_ = - this->declare_parameter("stop_status_topic", "/stop_status"); + this->declare_parameter("stop_status_topic", "stop_status"); // Subscriber topics dock_subscription_topic_ = - this->declare_parameter("dock_topic", "/dock_status"); + this->declare_parameter("dock_topic", "dock_status"); wheel_vels_subscription_topic_ = - this->declare_parameter("wheel_vels_topic", "/odom"); + this->declare_parameter("wheel_vels_topic", "odom"); // Publish rate parameters in Hz const double battery_state_publish_rate = diff --git a/irobot_create_common/irobot_create_nodes/src/ui_mgr.cpp b/irobot_create_common/irobot_create_nodes/src/ui_mgr.cpp index 730acdaf..cd1a9ed1 100644 --- a/irobot_create_common/irobot_create_nodes/src/ui_mgr.cpp +++ b/irobot_create_common/irobot_create_nodes/src/ui_mgr.cpp @@ -28,13 +28,13 @@ UIMgr::UIMgr(const rclcpp::NodeOptions & options) // Topic parameter to publish buttons to buttons_publisher_topic_ = - this->declare_parameter("button_topic", "/interface_buttons"); + this->declare_parameter("button_topic", "interface_buttons"); // Subscriber topics lightring_subscription_topic_ = - this->declare_parameter("lightring_topic", "/cmd_lightring"); + this->declare_parameter("lightring_topic", "cmd_lightring"); audio_subscription_topic_ = - this->declare_parameter("audio_topic", "/cmd_audio"); + this->declare_parameter("audio_topic", "cmd_audio"); // Publish rate parameters in Hz const double buttons_publish_rate = diff --git a/irobot_create_common/irobot_create_nodes/src/wheels_publisher.cpp b/irobot_create_common/irobot_create_nodes/src/wheels_publisher.cpp index 0893ecd2..56e72233 100644 --- a/irobot_create_common/irobot_create_nodes/src/wheels_publisher.cpp +++ b/irobot_create_common/irobot_create_nodes/src/wheels_publisher.cpp @@ -16,11 +16,11 @@ WheelsPublisher::WheelsPublisher(const rclcpp::NodeOptions & options) { // Topic parameter to publish angular velocity to const std::string velocity_topic = - this->declare_parameter("velocity_topic", "/wheel_vels"); + this->declare_parameter("velocity_topic", "wheel_vels"); // Topic parameter to publish wheel ticks to const std::string ticks_topic = - this->declare_parameter("ticks_topic", "/wheel_ticks"); + this->declare_parameter("ticks_topic", "wheel_ticks"); // Publish rate parameter in Hz const double publish_rate = diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/config/pose_republisher_params.yaml b/irobot_create_ignition/irobot_create_ignition_bringup/config/pose_republisher_params.yaml index 8cdb5e3b..e31c92ee 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/config/pose_republisher_params.yaml +++ b/irobot_create_ignition/irobot_create_ignition_bringup/config/pose_republisher_params.yaml @@ -1,10 +1,10 @@ --- pose_republisher_node: ros__parameters: - robot_publisher_topic: /sim_ground_truth_pose - robot_subscriber_topic: /_internal/sim_ground_truth_pose - mouse_publisher_topic: /_internal/sim_ground_truth_mouse_pose - dock_subscriber_topic: /_internal/sim_ground_truth_dock_pose - dock_publisher_topic: /sim_ground_truth_dock_pose - ir_emitter_publisher_topic: /_internal/sim_ground_truth_ir_emitter_pose - ir_receiver_publisher_topic: /_internal/sim_ground_truth_ir_receiver_pose \ No newline at end of file + robot_publisher_topic: sim_ground_truth_pose + robot_subscriber_topic: _internal/sim_ground_truth_pose + mouse_publisher_topic: _internal/sim_ground_truth_mouse_pose + dock_subscriber_topic: _internal/sim_ground_truth_dock_pose + dock_publisher_topic: sim_ground_truth_dock_pose + ir_emitter_publisher_topic: _internal/sim_ground_truth_ir_emitter_pose + ir_receiver_publisher_topic: _internal/sim_ground_truth_ir_receiver_pose \ No newline at end of file diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py index 057c34b3..82c21364 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py @@ -44,7 +44,7 @@ def perform( description='Ignition World'), DeclareLaunchArgument('robot_name', default_value='create3', description='Robot name'), - DeclareLaunchArgument('use_rviz', default_value='true', + DeclareLaunchArgument('use_rviz', default_value='false', choices=['true', 'false'], description='Start rviz.'), DeclareLaunchArgument('spawn_dock', default_value='true', choices=['true', 'false'], @@ -87,30 +87,16 @@ def generate_launch_description(): # Paths ign_gazebo_launch = PathJoinSubstitution( [pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py']) - ros_ign_bridge_launch = PathJoinSubstitution( - [pkg_irobot_create_ignition_bringup, 'launch', 'create3_ros_ignition_bridge.launch.py']) - create3_nodes_launch = PathJoinSubstitution( - [pkg_irobot_create_common_bringup, 'launch', 'create3_nodes.launch.py']) - create3_ignition_nodes_launch = PathJoinSubstitution( - [pkg_irobot_create_ignition_bringup, 'launch', 'create3_ignition_nodes.launch.py']) - robot_description_launch = PathJoinSubstitution( - [pkg_irobot_create_common_bringup, 'launch', 'robot_description.launch.py']) - dock_description_launch = PathJoinSubstitution( - [pkg_irobot_create_common_bringup, 'launch', 'dock_description.launch.py']) rviz2_launch = PathJoinSubstitution( [pkg_irobot_create_common_bringup, 'launch', 'rviz2.launch.py']) - # Launch configurations - x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') - yaw = LaunchConfiguration('yaw') - # Ignition gazebo ignition_gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([ign_gazebo_launch]), launch_arguments=[ ('ign_args', [LaunchConfiguration('world'), '.sdf', - ' -v 4', + ' -v 6', ' --gui-config ', PathJoinSubstitution([pkg_irobot_create_ignition_bringup, 'gui', 'create3', 'gui.config'])]) @@ -122,69 +108,19 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('use_rviz')), ) - x_dock = OffsetParser(x, 0.157) - yaw_dock = OffsetParser(yaw, 3.1416) - dock_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource([dock_description_launch]), - condition=IfCondition(LaunchConfiguration('spawn_dock')), - # The robot starts docked - launch_arguments={'x': x_dock, 'y': y, 'z': z, 'yaw': yaw_dock, - 'gazebo': 'ignition'}.items(), - ) - - robot_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource([robot_description_launch]), - launch_arguments={'gazebo': 'ignition'}.items() - ) - - # Create3 - spawn_robot = Node(package='ros_ign_gazebo', executable='create', - arguments=['-name', LaunchConfiguration('robot_name'), - '-x', x, - '-y', y, - '-z', z, - '-Y', '0.0', - '-topic', 'robot_description'], - output='screen') - - # Dock - spawn_dock = Node(package='ros_ign_gazebo', executable='create', - arguments=['-name', 'standard_dock', - '-x', x_dock, - '-y', y, - '-z', z, - '-Y', '3.141592', - '-topic', 'standard_dock_description'], - output='screen') - - # ROS Ign bridge - ros_ign_bridge = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ros_ign_bridge_launch]), - launch_arguments=[('world', LaunchConfiguration('world')), - ('robot_name', LaunchConfiguration('robot_name'))] - ) - - # Create3 nodes - create3_nodes = IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_nodes_launch]) - ) - - create3_ignition_nodes = IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), - launch_arguments=[('robot_name', LaunchConfiguration('robot_name'))] - ) + # clock bridge + clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' + ]) # Create launch description and add actions ld = LaunchDescription(ARGUMENTS) ld.add_action(ign_resource_path) ld.add_action(ign_gui_plugin_path) ld.add_action(ignition_gazebo) - ld.add_action(ros_ign_bridge) + ld.add_action(clock_bridge) ld.add_action(rviz2) - ld.add_action(robot_description) - ld.add_action(dock_description) - ld.add_action(spawn_robot) - ld.add_action(spawn_dock) - ld.add_action(create3_nodes) - ld.add_action(create3_ignition_nodes) return ld diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py index 21f3488f..8b4cef59 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py @@ -8,6 +8,8 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + ARGUMENTS = [ DeclareLaunchArgument('robot_name', default_value='create3', description='Robot name') @@ -18,18 +20,31 @@ def generate_launch_description(): # Directories pkg_create3_ignition_bringup = get_package_share_directory('irobot_create_ignition_bringup') + namespace = LaunchConfiguration('namespace', default='') pose_republisher_params_yaml_file = PathJoinSubstitution( [pkg_create3_ignition_bringup, 'config', 'pose_republisher_params.yaml']) sensors_params_yaml_file = PathJoinSubstitution( [pkg_create3_ignition_bringup, 'config', 'sensors_params.yaml']) + namespaced_pose_republisher_params_yaml_file = RewrittenYaml( + source_file=pose_republisher_params_yaml_file, + root_key=namespace, + param_rewrites={}, + convert_types=True) + + namespaced_sensors_params_yaml_file = RewrittenYaml( + source_file=sensors_params_yaml_file, + root_key=namespace, + param_rewrites={}, + convert_types=True) + # Pose republisher pose_republisher_node = Node( package='irobot_create_ignition_toolbox', name='pose_republisher_node', executable='pose_republisher_node', - parameters=[pose_republisher_params_yaml_file, + parameters=[namespaced_pose_republisher_params_yaml_file, {'robot_name': LaunchConfiguration('robot_name')}, {'use_sim_time': True}], output='screen', @@ -40,7 +55,7 @@ def generate_launch_description(): package='irobot_create_ignition_toolbox', name='sensors_node', executable='sensors_node', - parameters=[sensors_params_yaml_file, + parameters=[namespaced_sensors_params_yaml_file, {'use_sim_time': True}], output='screen', ) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py index c3997cb5..c3da0871 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py @@ -3,7 +3,6 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -40,18 +39,8 @@ def generate_launch_description(): 'ir_intensity_side_left', ] - # clock bridge - clock_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - namespace=namespace, - name='clock_bridge', - output='screen', - arguments=[ - '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' - ], - condition=IfCondition(use_sim_time)) - # cmd_vel bridge - cmd_vel_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', + cmd_vel_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', name='cmd_vel_bridge', output='screen', parameters=[{ @@ -64,13 +53,13 @@ def generate_launch_description(): ']ignition.msgs.Twist'] ], remappings=[ + ('/cmd_vel', 'cmd_vel'), (['/model/', LaunchConfiguration('robot_name'), '/cmd_vel'], 'diffdrive_controller/cmd_vel_unstamped') ]) # Pose bridge - pose_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - namespace=namespace, + pose_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', name='pose_bridge', output='screen', parameters=[{ @@ -86,14 +75,13 @@ def generate_launch_description(): ], remappings=[ (['/model/', LaunchConfiguration('robot_name'), '/pose'], - '/_internal/sim_ground_truth_pose'), + '_internal/sim_ground_truth_pose'), ('/model/standard_dock/pose', - '/_internal/sim_ground_truth_dock_pose') + '_internal/sim_ground_truth_dock_pose') ]) # odom to base_link transform bridge - odom_base_tf_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - namespace=namespace, + odom_base_tf_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', name='odom_base_tf_bridge', output='screen', parameters=[{ @@ -105,12 +93,11 @@ def generate_launch_description(): '[ignition.msgs.Pose_V'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/tf'], '/tf') + (['/model/', LaunchConfiguration('robot_name'), '/tf'], 'tf') ]) # Bumper contact sensor bridge - bumper_contact_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - namespace=namespace, + bumper_contact_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', name='bumper_contact_bridge', output='screen', parameters=[{ @@ -125,12 +112,11 @@ def generate_launch_description(): remappings=[ (['/model/', LaunchConfiguration('robot_name'), '/bumper_contact'], - '/bumper_contact') + 'bumper_contact') ]) # Cliff bridge - cliff_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - namespace=namespace, + cliff_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', name='cliff_bridge', output='screen', parameters=[{ @@ -147,12 +133,12 @@ def generate_launch_description(): (['/world/', LaunchConfiguration('world'), '/model/', LaunchConfiguration('robot_name'), '/link/base_link/sensor/' + cliff + '/scan'], - '/_internal/' + cliff + '/scan') + '_internal/' + cliff + '/scan') for cliff in cliff_sensors ]) + # IR intensity bridge - ir_intensity_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - namespace=namespace, + ir_intensity_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', name='ir_intensity_bridge', output='screen', parameters=[{ @@ -169,12 +155,11 @@ def generate_launch_description(): (['/world/', LaunchConfiguration('world'), '/model/', LaunchConfiguration('robot_name'), '/link/' + ir + '/sensor/' + ir + '/scan'], - '/_internal/' + ir + '/scan') for ir in ir_intensity_sensors + '_internal/' + ir + '/scan') for ir in ir_intensity_sensors ]) # Buttons message bridge - buttons_msg_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - namespace=namespace, + buttons_msg_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', name='buttons_msg_bridge', output='screen', parameters=[{ @@ -184,11 +169,13 @@ def generate_launch_description(): ['/create3/buttons' + '@std_msgs/msg/Int32' + '[ignition.msgs.Int32'] + ], + remappings=[ + ('/create3/buttons', 'create3_buttons') ]) # Create launch description and add actions ld = LaunchDescription(ARGUMENTS) - ld.add_action(clock_bridge) ld.add_action(cmd_vel_bridge) ld.add_action(pose_bridge) ld.add_action(odom_base_tf_bridge) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn_robot.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn_robot.launch.py new file mode 100644 index 00000000..442f4044 --- /dev/null +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn_robot.launch.py @@ -0,0 +1,156 @@ +# Copyright 2021 Clearpath Robotics, Inc. +# @author Roni Kreinin (rkreinin@clearpathrobotics.com) + +import os + +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.actions import IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node, PushRosNamespace + + +class OffsetParser(Substitution): + def __init__( + self, + number: SomeSubstitutionsType, + offset: float, + ) -> None: + self.__number = number + self.__offset = offset + + def perform( + self, + context: LaunchContext = None, + ) -> str: + number = float(self.__number.perform(context)) + return f'{number + self.__offset}' + + +ARGUMENTS = [ + DeclareLaunchArgument('bridge', default_value='true', + choices=['true', 'false'], + description='Use ros_ign_bridge'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('world', default_value='depot', + description='Ignition World'), + DeclareLaunchArgument('robot_name', default_value='create3', + description='Robot name'), + DeclareLaunchArgument('use_rviz', default_value='true', + choices=['true', 'false'], description='Start rviz.'), + DeclareLaunchArgument('spawn_dock', default_value='true', + choices=['true', 'false'], + description='Spawn the standard dock model.'), +] + +for pose_element in ['x', 'y', 'z', 'yaw']: + ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', + description=f'{pose_element} component of the robot pose.')) + + +def generate_launch_description(): + + # Directories + pkg_irobot_create_common_bringup = get_package_share_directory( + 'irobot_create_common_bringup') + pkg_irobot_create_ignition_bringup = get_package_share_directory( + 'irobot_create_ignition_bringup') + + # Paths + ros_ign_bridge_launch = PathJoinSubstitution( + [pkg_irobot_create_ignition_bringup, 'launch', 'create3_ros_ignition_bridge.launch.py']) + create3_nodes_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'create3_nodes.launch.py']) + create3_ignition_nodes_launch = PathJoinSubstitution( + [pkg_irobot_create_ignition_bringup, 'launch', 'create3_ignition_nodes.launch.py']) + robot_description_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'robot_description.launch.py']) + dock_description_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'dock_description.launch.py']) + + # Launch configurations + namespace = LaunchConfiguration('robot_name') + x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') + yaw = LaunchConfiguration('yaw') + + x_dock = OffsetParser(x, 0.157) + yaw_dock = OffsetParser(yaw, 3.1416) + + spawn_robot_group_action = GroupAction([ + PushRosNamespace(namespace), + + # Dock description + IncludeLaunchDescription( + PythonLaunchDescriptionSource([dock_description_launch]), + condition=IfCondition(LaunchConfiguration('spawn_dock')), + # The robot starts docked + launch_arguments={'x': x_dock, 'y': y, 'z': z, 'yaw': yaw_dock, + 'namespace': namespace, + 'gazebo': 'ignition'}.items(), + ), + + # Robot description + IncludeLaunchDescription( + PythonLaunchDescriptionSource([robot_description_launch]), + launch_arguments={'gazebo': 'ignition', + 'robot_name': LaunchConfiguration('robot_name')}.items() + ), + + # Spawn Create 3 + Node( + package='ros_ign_gazebo', + executable='create', + arguments=['-name', LaunchConfiguration('robot_name'), + '-x', x, + '-y', y, + '-z', z, + '-Y', '0.0', + '-topic', 'robot_description'], + output='screen' + ), + + # Spawn dock + Node( + package='ros_ign_gazebo', + executable='create', + arguments=['-name', [LaunchConfiguration('robot_name'), '/standard_dock'], + '-x', x_dock, + '-y', y, + '-z', z, + '-Y', '3.141592', + '-topic', 'standard_dock_description'], + output='screen', + ), + + # ROS Ign Bridge + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ros_ign_bridge_launch]), + launch_arguments=[('world', LaunchConfiguration('world')), + ('robot_name', LaunchConfiguration('robot_name'))] + ), + + # Create 3 nodes + IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_nodes_launch]), + launch_arguments=[('robot_name', LaunchConfiguration('robot_name'))] + ), + + # Create 3 Ignition nodes + IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), + launch_arguments=[('robot_name', LaunchConfiguration('robot_name'))] + ) + ]) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(spawn_robot_group_action) + return ld diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp index 62442f82..f5b751ca 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp @@ -12,7 +12,7 @@ InterfaceButtons::InterfaceButtons() : rclcpp::Node("sensors_node") { interface_buttons_sub_ = this->create_subscription( - "create3/buttons", + "create3_buttons", rclcpp::SensorDataQoS(), std::bind(&InterfaceButtons::create3_buttons_callback, this, std::placeholders::_1)); diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp index 29b3b4e7..a4601eaa 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp @@ -18,20 +18,20 @@ PoseRepublisher::PoseRepublisher() robot_name_ = this->declare_parameter("robot_name", "create3"); std::string robot_pub_topic = - this->declare_parameter("robot_publisher_topic", "/sim_ground_truth_pose"); + this->declare_parameter("robot_publisher_topic", "sim_ground_truth_pose"); std::string robot_sub_topic = - this->declare_parameter("robot_subscriber_topic", "/sim_ground_truth_pose"); + this->declare_parameter("robot_subscriber_topic", "sim_ground_truth_pose"); std::string mouse_pub_topic = - this->declare_parameter("mouse_publisher_topic", "/sim_ground_truth_mouse_pose"); + this->declare_parameter("mouse_publisher_topic", "sim_ground_truth_mouse_pose"); std::string dock_pub_topic = - this->declare_parameter("dock_publisher_topic", "/sim_ground_truth_dock_pose"); + this->declare_parameter("dock_publisher_topic", "sim_ground_truth_dock_pose"); std::string dock_sub_topic = - this->declare_parameter("dock_subscriber_topic", "/sim_ground_truth_dock_pose"); + this->declare_parameter("dock_subscriber_topic", "sim_ground_truth_dock_pose"); std::string ir_emitter_pub_topic = - this->declare_parameter("ir_emitter_publisher_topic", "/sim_ground_truth_ir_emitter_pose"); + this->declare_parameter("ir_emitter_publisher_topic", "sim_ground_truth_ir_emitter_pose"); std::string ir_receiver_pub_topic = - this->declare_parameter("ir_receiver_publisher_topic", "/sim_ground_truth_ir_receiver_pose"); + this->declare_parameter("ir_receiver_publisher_topic", "sim_ground_truth_ir_receiver_pose"); robot_subscriber_ = create_subscription( robot_sub_topic, diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_opcode.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_opcode.cpp index ab58962a..a30dc96f 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_opcode.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/ir_opcode.cpp @@ -15,12 +15,12 @@ IrOpcode::IrOpcode(std::shared_ptr & nh) : nh_(nh) { emitter_pose_sub_ = nh_->create_subscription( - "/_internal/sim_ground_truth_ir_emitter_pose", + "_internal/sim_ground_truth_ir_emitter_pose", rclcpp::SensorDataQoS(), std::bind(&IrOpcode::emitter_pose_callback, this, std::placeholders::_1)); receiver_pose_sub_ = nh_->create_subscription( - "/_internal/sim_ground_truth_ir_receiver_pose", + "_internal/sim_ground_truth_ir_receiver_pose", rclcpp::SensorDataQoS(), std::bind(&IrOpcode::receiver_pose_callback, this, std::placeholders::_1)); diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/mouse.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/mouse.cpp index ea578a87..e2d24f53 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/mouse.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/mouse.cpp @@ -16,7 +16,7 @@ Mouse::Mouse(std::shared_ptr & nh) last_mouse_position_{0, 0, 0} { mouse_pose_sub_ = nh_->create_subscription( - "/_internal/sim_ground_truth_mouse_pose", + "_internal/sim_ground_truth_mouse_pose", rclcpp::SensorDataQoS(), std::bind(&Mouse::mouse_pose_callback, this, std::placeholders::_1)); From 49889a79985d3d463ede372cf5fe9b3b27a93d1c Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Fri, 10 Mar 2023 11:54:32 -0500 Subject: [PATCH 02/14] Use wildcards for param files Added robot name to Create 3 buttons --- .../config/hazard_vector_params.yaml | 37 +++--- .../config/ir_intensity_vector_params.yaml | 29 ++--- .../config/kidnap_estimator_params.yaml | 13 +- .../config/mock_params.yaml | 13 +- .../config/robot_state_params.yaml | 43 +++---- .../config/ui_mgr_params.yaml | 19 +-- .../config/wheel_status_params.yaml | 25 ++-- .../launch/create3_nodes.launch.py | 4 +- .../launch/dock_description.launch.py | 3 +- .../launch/robot_description.launch.py | 16 +-- .../launch/include/control.py | 13 +- .../urdf/create3.urdf.xacro | 5 +- .../config/pose_republisher_params.yaml | 19 +-- .../config/sensors_params.yaml | 65 +++++----- .../gui/create3/gui.config | 1 + .../launch/create3_ignition.launch.py | 105 ++++------------ .../launch/create3_ignition_nodes.launch.py | 19 +-- .../create3_ros_ignition_bridge.launch.py | 115 +++++++++--------- ...obot.launch.py => create3_spawn.launch.py} | 3 +- .../launch/ignition.launch.py | 105 ++++++++++++++++ .../Create3Hmi/Create3Hmi.cc | 55 +++++++-- .../Create3Hmi/Create3Hmi.hh | 36 +++++- .../Create3Hmi/Create3Hmi.qml | 38 ++++-- .../interface_buttons_node.cpp | 2 +- 24 files changed, 448 insertions(+), 335 deletions(-) rename irobot_create_ignition/irobot_create_ignition_bringup/launch/{create3_spawn_robot.launch.py => create3_spawn.launch.py} (98%) create mode 100644 irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py diff --git a/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml index 2198ad0f..30c44cc9 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml @@ -1,19 +1,20 @@ --- -hazards_vector_publisher: - ros__parameters: - # Hazard detection publisher topic - publisher_topic: hazard_detection - publish_rate: 62.0 - subscription_topics: - # Bumper topic - - _internal/bumper/event - # Cliff topics - - _internal/cliff_front_left/event - - _internal/cliff_front_right/event - - _internal/cliff_side_left/event - - _internal/cliff_side_right/event - # Wheel drop topics - - _internal/wheel_drop/left_wheel/event - - _internal/wheel_drop/right_wheel/event - # Backup limit topic - - _internal/backup_limit +/**: + hazards_vector_publisher: + ros__parameters: + # Hazard detection publisher topic + publisher_topic: hazard_detection + publish_rate: 62.0 + subscription_topics: + # Bumper topic + - _internal/bumper/event + # Cliff topics + - _internal/cliff_front_left/event + - _internal/cliff_front_right/event + - _internal/cliff_side_left/event + - _internal/cliff_side_right/event + # Wheel drop topics + - _internal/wheel_drop/left_wheel/event + - _internal/wheel_drop/right_wheel/event + # Backup limit topic + - _internal/backup_limit diff --git a/irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml index b931647d..bf35b9bf 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml @@ -1,15 +1,16 @@ --- -ir_intensity_vector_publisher: - ros__parameters: - # IR intensity publisher topic - publisher_topic: ir_intensity - publish_rate: 62.0 - subscription_topics: - # IR intensity topics - - _internal/ir_intensity_front_center_left - - _internal/ir_intensity_front_center_right - - _internal/ir_intensity_front_left - - _internal/ir_intensity_front_right - - _internal/ir_intensity_left - - _internal/ir_intensity_right - - _internal/ir_intensity_side_left +/**: + ir_intensity_vector_publisher: + ros__parameters: + # IR intensity publisher topic + publisher_topic: ir_intensity + publish_rate: 62.0 + subscription_topics: + # IR intensity topics + - _internal/ir_intensity_front_center_left + - _internal/ir_intensity_front_center_right + - _internal/ir_intensity_front_left + - _internal/ir_intensity_front_right + - _internal/ir_intensity_left + - _internal/ir_intensity_right + - _internal/ir_intensity_side_left diff --git a/irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml index 9e8d3caf..84b334bd 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml @@ -1,7 +1,8 @@ --- -kidnap_estimator_publisher: - ros__parameters: - # Kidnap status publisher topic - kidnap_status_topic: kidnap_status - # Subscription topics - hazard_topic: hazard_detection +/**: + kidnap_estimator_publisher: + ros__parameters: + # Kidnap status publisher topic + kidnap_status_topic: kidnap_status + # Subscription topics + hazard_topic: hazard_detection diff --git a/irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml index 8cea8f91..fa28a80c 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml @@ -1,7 +1,8 @@ --- -mock_publisher: - ros__parameters: - # Mock slip status publisher topic - slip_status_topic: slip_status - # Publishers rate - slip_status_publish_rate: 62.0 +/**: + mock_publisher: + ros__parameters: + # Mock slip status publisher topic + slip_status_topic: slip_status + # Publishers rate + slip_status_publish_rate: 62.0 diff --git a/irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml index a182c38f..ac63e386 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml @@ -1,22 +1,23 @@ --- -robot_state: - ros__parameters: - # Stop status publisher topic - stop_status_topic: stop_status - # Mock battery state publisher topic - battery_state_topic: battery_state - # Publishers rate - kidnap_status_publish_rate: 1.0 - stop_status_publish_rate: 62.0 - battery_state_publish_rate: 0.1 - # Subscription topics - dock_topic: dock_status - wheel_vels_topic: odom - # Stop status position difference tolerance - linear_velocity_tolerance: 0.01 - angular_velocity_tolerance: 0.1 - # Battery parameters - full_charge_percentage: 1.0 - battery_high_percentage: 0.9 - # Dock Parameters - undocked_charge_limit: 0.03 +/**: + robot_state: + ros__parameters: + # Stop status publisher topic + stop_status_topic: stop_status + # Mock battery state publisher topic + battery_state_topic: battery_state + # Publishers rate + kidnap_status_publish_rate: 1.0 + stop_status_publish_rate: 62.0 + battery_state_publish_rate: 0.1 + # Subscription topics + dock_topic: dock_status + wheel_vels_topic: odom + # Stop status position difference tolerance + linear_velocity_tolerance: 0.01 + angular_velocity_tolerance: 0.1 + # Battery parameters + full_charge_percentage: 1.0 + battery_high_percentage: 0.9 + # Dock Parameters + undocked_charge_limit: 0.03 diff --git a/irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml index 3298d059..b5d3aa21 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml @@ -1,10 +1,11 @@ --- -ui_mgr: - ros__parameters: - # Buttons publisher topic - button_topic: interface_buttons - # Publishers rate - buttons_publish_rate: 1.0 - # Subscription topics - lightring_topic: cmd_lightring - audio_topic: cmd_audio +/**: + ui_mgr: + ros__parameters: + # Buttons publisher topic + button_topic: interface_buttons + # Publishers rate + buttons_publish_rate: 1.0 + # Subscription topics + lightring_topic: cmd_lightring + audio_topic: cmd_audio diff --git a/irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml index 168e828b..ad354d4e 100644 --- a/irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml +++ b/irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml @@ -1,13 +1,14 @@ --- -wheel_status_publisher: - ros__parameters: - # Publish rate - publish_rate: 62.0 - # Encoder resolution - encoder_resolution: 508.8 - # Wheel radius - wheel_radius: 0.03575 - # Wheels angular velocity topic - velocity_topic: wheel_vels - # Wheels' net encoder ticks topic - ticks_topic: wheel_ticks +/**: + wheel_status_publisher: + ros__parameters: + # Publish rate + publish_rate: 62.0 + # Encoder resolution + encoder_resolution: 508.8 + # Wheel radius + wheel_radius: 0.03575 + # Wheels angular velocity topic + velocity_topic: wheel_vels + # Wheels' net encoder ticks topic + ticks_topic: wheel_ticks diff --git a/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py index 4fff821f..ce5cd68e 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py @@ -14,7 +14,9 @@ ARGUMENTS = [ DeclareLaunchArgument('gazebo', default_value='classic', choices=['classic', 'ignition'], - description='Which gazebo simulator to use') + description='Which gazebo simulator to use'), + DeclareLaunchArgument('robot_name', default_value='create3', + description='Robot name'), ] diff --git a/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py index 4d0fc371..4a65fe71 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py @@ -37,7 +37,6 @@ def generate_launch_description(): visualize_rays = LaunchConfiguration('visualize_rays') gazebo_simulator = LaunchConfiguration('gazebo') - namespace = LaunchConfiguration('namespace', default='') state_publisher = Node( package='robot_state_publisher', @@ -67,7 +66,7 @@ def generate_launch_description(): # According to documentation (http://wiki.ros.org/tf2_ros): # the order is yaw, pitch, roll yaw, '0', '0', - [namespace, '/odom'], [namespace, '/std_dock_link']], + 'odom', 'std_dock_link'], remappings=[ ('/tf', 'tf'), ('/tf_static', 'tf_static') diff --git a/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py index 3009ce52..80f2bfd2 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py @@ -10,7 +10,6 @@ from launch.substitutions.launch_configuration import LaunchConfiguration from launch_ros.actions import Node -from nav2_common.launch import RewrittenYaml ARGUMENTS = [ DeclareLaunchArgument('gazebo', default_value='classic', @@ -26,20 +25,10 @@ def generate_launch_description(): pkg_create3_description = get_package_share_directory('irobot_create_description') - pkg_create3_control = get_package_share_directory('irobot_create_control') xacro_file = PathJoinSubstitution([pkg_create3_description, 'urdf', 'create3.urdf.xacro']) gazebo_simulator = LaunchConfiguration('gazebo') visualize_rays = LaunchConfiguration('visualize_rays') - namespace = LaunchConfiguration('robot_name') - - control_params_file = PathJoinSubstitution( - [pkg_create3_control, 'config', 'control.yaml']) - - namespaced_control_params_file = RewrittenYaml( - source_file=control_params_file, - root_key=namespace, - param_rewrites={}, - ) + robot_name = LaunchConfiguration('robot_name') robot_state_publisher = Node( package='robot_state_publisher', @@ -53,8 +42,7 @@ def generate_launch_description(): ['xacro', ' ', xacro_file, ' ', 'gazebo:=', gazebo_simulator, ' ', 'visualize_rays:=', visualize_rays, ' ', - 'namespace:=', namespace, ' ', - 'control_params:=', control_params_file])}, + 'namespace:=', robot_name])}, ], remappings=[ ('/tf', 'tf'), diff --git a/irobot_create_common/irobot_create_control/launch/include/control.py b/irobot_create_common/irobot_create_control/launch/include/control.py index f1389ee0..4488f00b 100644 --- a/irobot_create_common/irobot_create_control/launch/include/control.py +++ b/irobot_create_common/irobot_create_control/launch/include/control.py @@ -5,11 +5,16 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import RegisterEventHandler +from launch.actions import DeclareLaunchArgument, RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +ARGUMENTS = [ + DeclareLaunchArgument('robot_name', default_value='create3', + description='Robot name'), +] + def generate_launch_description(): pkg_create3_control = get_package_share_directory('irobot_create_control') @@ -24,14 +29,14 @@ def generate_launch_description(): executable='spawner', namespace=robot_name, # Namespace is not pushed when used in EventHandler parameters=[control_params_file], - arguments=['diffdrive_controller', '-c', 'controller_manager'], + arguments=['diffdrive_controller', '-c', 'controller_manager', '-n', robot_name], output='screen', ) joint_state_broadcaster_spawner = Node( package='controller_manager', executable='spawner', - arguments=['joint_state_broadcaster', '-c', 'controller_manager'], + arguments=['joint_state_broadcaster', '-c', 'controller_manager', '-n', robot_name], output='screen', ) @@ -43,7 +48,7 @@ def generate_launch_description(): ) ) - ld = LaunchDescription() + ld = LaunchDescription(ARGUMENTS) ld.add_action(joint_state_broadcaster_spawner) ld.add_action(diffdrive_controller_callback) diff --git a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro index cec2b66e..0eeabd4f 100644 --- a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro @@ -15,10 +15,7 @@ - - - - + diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/config/pose_republisher_params.yaml b/irobot_create_ignition/irobot_create_ignition_bringup/config/pose_republisher_params.yaml index e31c92ee..f7a0c842 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/config/pose_republisher_params.yaml +++ b/irobot_create_ignition/irobot_create_ignition_bringup/config/pose_republisher_params.yaml @@ -1,10 +1,11 @@ --- -pose_republisher_node: - ros__parameters: - robot_publisher_topic: sim_ground_truth_pose - robot_subscriber_topic: _internal/sim_ground_truth_pose - mouse_publisher_topic: _internal/sim_ground_truth_mouse_pose - dock_subscriber_topic: _internal/sim_ground_truth_dock_pose - dock_publisher_topic: sim_ground_truth_dock_pose - ir_emitter_publisher_topic: _internal/sim_ground_truth_ir_emitter_pose - ir_receiver_publisher_topic: _internal/sim_ground_truth_ir_receiver_pose \ No newline at end of file +/**: + pose_republisher_node: + ros__parameters: + robot_publisher_topic: sim_ground_truth_pose + robot_subscriber_topic: _internal/sim_ground_truth_pose + mouse_publisher_topic: _internal/sim_ground_truth_mouse_pose + dock_subscriber_topic: _internal/sim_ground_truth_dock_pose + dock_publisher_topic: sim_ground_truth_dock_pose + ir_emitter_publisher_topic: _internal/sim_ground_truth_ir_emitter_pose + ir_receiver_publisher_topic: _internal/sim_ground_truth_ir_receiver_pose \ No newline at end of file diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/config/sensors_params.yaml b/irobot_create_ignition/irobot_create_ignition_bringup/config/sensors_params.yaml index 6b0ebba7..69c7715f 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/config/sensors_params.yaml +++ b/irobot_create_ignition/irobot_create_ignition_bringup/config/sensors_params.yaml @@ -1,37 +1,38 @@ --- -sensors_node: - ros__parameters: - cliff_subscription_topics: - - _internal/cliff_front_left/scan - - _internal/cliff_front_right/scan - - _internal/cliff_side_left/scan - - _internal/cliff_side_right/scan +/**: + sensors_node: + ros__parameters: + cliff_subscription_topics: + - _internal/cliff_front_left/scan + - _internal/cliff_front_right/scan + - _internal/cliff_side_left/scan + - _internal/cliff_side_right/scan - cliff_publish_topics: - - _internal/cliff_front_left/event - - _internal/cliff_front_right/event - - _internal/cliff_side_left/event - - _internal/cliff_side_right/event + cliff_publish_topics: + - _internal/cliff_front_left/event + - _internal/cliff_front_right/event + - _internal/cliff_side_left/event + - _internal/cliff_side_right/event - ir_scan_subscription_topics: - - _internal/ir_intensity_front_center_left/scan - - _internal/ir_intensity_front_center_right/scan - - _internal/ir_intensity_front_left/scan - - _internal/ir_intensity_front_right/scan - - _internal/ir_intensity_left/scan - - _internal/ir_intensity_right/scan - - _internal/ir_intensity_side_left/scan + ir_scan_subscription_topics: + - _internal/ir_intensity_front_center_left/scan + - _internal/ir_intensity_front_center_right/scan + - _internal/ir_intensity_front_left/scan + - _internal/ir_intensity_front_right/scan + - _internal/ir_intensity_left/scan + - _internal/ir_intensity_right/scan + - _internal/ir_intensity_side_left/scan - ir_intensity_publish_topics: - - _internal/ir_intensity_front_center_left - - _internal/ir_intensity_front_center_right - - _internal/ir_intensity_front_left - - _internal/ir_intensity_front_right - - _internal/ir_intensity_left - - _internal/ir_intensity_right - - _internal/ir_intensity_side_left + ir_intensity_publish_topics: + - _internal/ir_intensity_front_center_left + - _internal/ir_intensity_front_center_right + - _internal/ir_intensity_front_left + - _internal/ir_intensity_front_right + - _internal/ir_intensity_left + - _internal/ir_intensity_right + - _internal/ir_intensity_side_left - ir_opcode_sensor_0_fov: 3.839724 - ir_opcode_sensor_0_range: 0.1 - ir_opcode_sensor_1_fov: 1.570796 - ir_opcode_sensor_1_range: 0.5 \ No newline at end of file + ir_opcode_sensor_0_fov: 3.839724 + ir_opcode_sensor_0_range: 0.1 + ir_opcode_sensor_1_fov: 1.570796 + ir_opcode_sensor_1_range: 0.5 \ No newline at end of file diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config b/irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config index 615cecfb..626f4649 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config +++ b/irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config @@ -103,6 +103,7 @@ + /create3/cmd_vel true docked diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py index 82c21364..b0c1da5c 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py @@ -1,54 +1,23 @@ # Copyright 2021 Clearpath Robotics, Inc. # @author Roni Kreinin (rkreinin@clearpathrobotics.com) -import os - -from pathlib import Path - from ament_index_python.packages import get_package_share_directory -from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution +from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable -from launch.conditions import IfCondition +from launch.actions import IncludeLaunchDescription +from launch.conditions import IfCondition, LaunchConfigurationNotEquals from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node - - -class OffsetParser(Substitution): - def __init__( - self, - number: SomeSubstitutionsType, - offset: float, - ) -> None: - self.__number = number - self.__offset = offset - - def perform( - self, - context: LaunchContext = None, - ) -> str: - number = float(self.__number.perform(context)) - return f'{number + self.__offset}' ARGUMENTS = [ - DeclareLaunchArgument('bridge', default_value='true', - choices=['true', 'false'], - description='Use ros_ign_bridge'), - DeclareLaunchArgument('use_sim_time', default_value='true', - choices=['true', 'false'], - description='use_sim_time'), - DeclareLaunchArgument('world', default_value='depot', - description='Ignition World'), DeclareLaunchArgument('robot_name', default_value='create3', description='Robot name'), DeclareLaunchArgument('use_rviz', default_value='false', choices=['true', 'false'], description='Start rviz.'), - DeclareLaunchArgument('spawn_dock', default_value='true', - choices=['true', 'false'], - description='Spawn the standard dock model.'), + DeclareLaunchArgument('world', default_value='depot', + description='Ignition World'), ] for pose_element in ['x', 'y', 'z', 'yaw']: @@ -59,47 +28,23 @@ def perform( def generate_launch_description(): # Directories - pkg_irobot_create_common_bringup = get_package_share_directory( - 'irobot_create_common_bringup') pkg_irobot_create_ignition_bringup = get_package_share_directory( 'irobot_create_ignition_bringup') - pkg_irobot_create_ignition_plugins = get_package_share_directory( - 'irobot_create_ignition_plugins') - pkg_irobot_create_description = get_package_share_directory( - 'irobot_create_description') - pkg_ros_ign_gazebo = get_package_share_directory( - 'ros_ign_gazebo') - - # Set Ignition resource path - ign_resource_path = SetEnvironmentVariable(name='IGN_GAZEBO_RESOURCE_PATH', - value=[os.path.join( - pkg_irobot_create_ignition_bringup, - 'worlds'), ':' + - str(Path( - pkg_irobot_create_description). - parent.resolve())]) - - ign_gui_plugin_path = SetEnvironmentVariable(name='IGN_GUI_PLUGIN_PATH', - value=[os.path.join( - pkg_irobot_create_ignition_plugins, - 'lib')]) + pkg_irobot_create_common_bringup = get_package_share_directory( + 'irobot_create_common_bringup') # Paths - ign_gazebo_launch = PathJoinSubstitution( - [pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py']) + ignition_launch = PathJoinSubstitution( + [pkg_irobot_create_ignition_bringup, 'launch', 'ignition.launch.py']) rviz2_launch = PathJoinSubstitution( [pkg_irobot_create_common_bringup, 'launch', 'rviz2.launch.py']) + robot_spawn_launch = PathJoinSubstitution( + [pkg_irobot_create_ignition_bringup, 'launch', 'create3_spawn.launch.py']) - # Ignition gazebo - ignition_gazebo = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ign_gazebo_launch]), + ignition = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ignition_launch]), launch_arguments=[ - ('ign_args', [LaunchConfiguration('world'), - '.sdf', - ' -v 6', - ' --gui-config ', - PathJoinSubstitution([pkg_irobot_create_ignition_bringup, - 'gui', 'create3', 'gui.config'])]) + ('world', LaunchConfiguration('world')) ] ) @@ -108,19 +53,19 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('use_rviz')), ) - # clock bridge - clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', - name='clock_bridge', - output='screen', - arguments=[ - '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' - ]) + robot_spawn = IncludeLaunchDescription( + PythonLaunchDescriptionSource([robot_spawn_launch]), + launch_arguments=[ + ('robot_name', LaunchConfiguration('robot_name')), + ('x', LaunchConfiguration('x')), + ('y', LaunchConfiguration('y')), + ('z', LaunchConfiguration('z')), + ('yaw', LaunchConfiguration('yaw'))], + condition=LaunchConfigurationNotEquals('robot_name', '')) # Create launch description and add actions ld = LaunchDescription(ARGUMENTS) - ld.add_action(ign_resource_path) - ld.add_action(ign_gui_plugin_path) - ld.add_action(ignition_gazebo) - ld.add_action(clock_bridge) + ld.add_action(ignition) ld.add_action(rviz2) + ld.add_action(robot_spawn) return ld diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py index 8b4cef59..21f3488f 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py @@ -8,8 +8,6 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node -from nav2_common.launch import RewrittenYaml - ARGUMENTS = [ DeclareLaunchArgument('robot_name', default_value='create3', description='Robot name') @@ -20,31 +18,18 @@ def generate_launch_description(): # Directories pkg_create3_ignition_bringup = get_package_share_directory('irobot_create_ignition_bringup') - namespace = LaunchConfiguration('namespace', default='') pose_republisher_params_yaml_file = PathJoinSubstitution( [pkg_create3_ignition_bringup, 'config', 'pose_republisher_params.yaml']) sensors_params_yaml_file = PathJoinSubstitution( [pkg_create3_ignition_bringup, 'config', 'sensors_params.yaml']) - namespaced_pose_republisher_params_yaml_file = RewrittenYaml( - source_file=pose_republisher_params_yaml_file, - root_key=namespace, - param_rewrites={}, - convert_types=True) - - namespaced_sensors_params_yaml_file = RewrittenYaml( - source_file=sensors_params_yaml_file, - root_key=namespace, - param_rewrites={}, - convert_types=True) - # Pose republisher pose_republisher_node = Node( package='irobot_create_ignition_toolbox', name='pose_republisher_node', executable='pose_republisher_node', - parameters=[namespaced_pose_republisher_params_yaml_file, + parameters=[pose_republisher_params_yaml_file, {'robot_name': LaunchConfiguration('robot_name')}, {'use_sim_time': True}], output='screen', @@ -55,7 +40,7 @@ def generate_launch_description(): package='irobot_create_ignition_toolbox', name='sensors_node', executable='sensors_node', - parameters=[namespaced_sensors_params_yaml_file, + parameters=[sensors_params_yaml_file, {'use_sim_time': True}], output='screen', ) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py index c3da0871..9bbf1fba 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py @@ -2,7 +2,7 @@ # @author Roni Kreinin (rkreinin@clearpathrobotics.com) from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, GroupAction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -19,8 +19,9 @@ def generate_launch_description(): - namespace = LaunchConfiguration('robot_name') use_sim_time = LaunchConfiguration('use_sim_time') + robot_name = LaunchConfiguration('robot_name') + world = LaunchConfiguration('world') cliff_sensors = [ 'cliff_front_left', @@ -47,14 +48,15 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - '/cmd_vel' + '@geometry_msgs/msg/Twist' + '[ignition.msgs.Twist', - ['/model/', LaunchConfiguration('robot_name'), '/cmd_vel' + + [robot_name, + '/cmd_vel' + '@geometry_msgs/msg/Twist' + '[ignition.msgs.Twist'], + ['/model/', robot_name, '/cmd_vel' + '@geometry_msgs/msg/Twist' + ']ignition.msgs.Twist'] ], remappings=[ - ('/cmd_vel', 'cmd_vel'), - (['/model/', LaunchConfiguration('robot_name'), '/cmd_vel'], + ([robot_name, '/cmd_vel'], 'cmd_vel'), + (['/model/', robot_name, '/cmd_vel'], 'diffdrive_controller/cmd_vel_unstamped') ]) @@ -66,7 +68,7 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), '/pose' + + ['/model/', robot_name, '/pose' + '@tf2_msgs/msg/TFMessage' + '[ignition.msgs.Pose_V'], '/model/standard_dock/pose' + @@ -74,7 +76,7 @@ def generate_launch_description(): '[ignition.msgs.Pose_V' ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/pose'], + (['/model/', robot_name, '/pose'], '_internal/sim_ground_truth_pose'), ('/model/standard_dock/pose', '_internal/sim_ground_truth_dock_pose') @@ -88,12 +90,12 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), '/tf' + + ['/model/', robot_name, '/tf' + '@tf2_msgs/msg/TFMessage' + '[ignition.msgs.Pose_V'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/tf'], 'tf') + (['/model/', robot_name, '/tf'], 'tf') ]) # Bumper contact sensor bridge @@ -104,59 +106,60 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), + ['/model/', robot_name, '/bumper_contact' + '@ros_ign_interfaces/msg/Contacts' + '[ignition.msgs.Contacts'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), + (['/model/', robot_name, '/bumper_contact'], 'bumper_contact') ]) # Cliff bridge - cliff_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', - name='cliff_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[ - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), - '/link/base_link/sensor/' + cliff + '/scan' + - '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] - for cliff in cliff_sensors - ], - remappings=[ - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), - '/link/base_link/sensor/' + cliff + '/scan'], - '_internal/' + cliff + '/scan') - for cliff in cliff_sensors - ]) + cliff_bridges = GroupAction([ + Node(package='ros_gz_bridge', executable='parameter_bridge', + name=cliff + '_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[ + ['/world/', world, + '/model/', robot_name, + '/link/base_link/sensor/' + cliff + '/scan' + + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ], + remappings=[ + (['/world/', world, + '/model/', robot_name, + '/link/base_link/sensor/' + cliff + '/scan'], + '_internal/' + cliff + '/scan') + ]) for cliff in cliff_sensors + ]) - # IR intensity bridge - ir_intensity_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', - name='ir_intensity_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[ - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), - '/link/' + ir + '/sensor/' + ir + '/scan' + - '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] - for ir in ir_intensity_sensors - ], - remappings=[ - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), - '/link/' + ir + '/sensor/' + ir + '/scan'], - '_internal/' + ir + '/scan') for ir in ir_intensity_sensors - ]) + # IR intensity bridges + ir_bridges = GroupAction([ + Node(package='ros_gz_bridge', executable='parameter_bridge', + name=ir + '_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[ + ['/world/', world, + '/model/', robot_name, + '/link/' + ir + '/sensor/' + ir + '/scan' + + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ], + remappings=[ + (['/world/', world, + '/model/', robot_name, + '/link/' + ir + '/sensor/' + ir + '/scan'], + '_internal/' + ir + '/scan') + ]) for ir in ir_intensity_sensors + ]) # Buttons message bridge buttons_msg_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', @@ -166,12 +169,12 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/create3/buttons' + + ['/model/', robot_name, '/create3_buttons' + '@std_msgs/msg/Int32' + '[ignition.msgs.Int32'] ], remappings=[ - ('/create3/buttons', 'create3_buttons') + (['/model/', robot_name, '/create3_buttons'], '_create3_buttons') ]) # Create launch description and add actions @@ -180,7 +183,7 @@ def generate_launch_description(): ld.add_action(pose_bridge) ld.add_action(odom_base_tf_bridge) ld.add_action(bumper_contact_bridge) - ld.add_action(cliff_bridge) - ld.add_action(ir_intensity_bridge) + ld.add_action(cliff_bridges) + ld.add_action(ir_bridges) ld.add_action(buttons_msg_bridge) return ld diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn_robot.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py similarity index 98% rename from irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn_robot.launch.py rename to irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py index 442f4044..98e7eab7 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn_robot.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py @@ -93,7 +93,6 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('spawn_dock')), # The robot starts docked launch_arguments={'x': x_dock, 'y': y, 'z': z, 'yaw': yaw_dock, - 'namespace': namespace, 'gazebo': 'ignition'}.items(), ), @@ -121,7 +120,7 @@ def generate_launch_description(): Node( package='ros_ign_gazebo', executable='create', - arguments=['-name', [LaunchConfiguration('robot_name'), '/standard_dock'], + arguments=['-name', [LaunchConfiguration('robot_name'), '_standard_dock'], '-x', x_dock, '-y', y, '-z', z, diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py new file mode 100644 index 00000000..fd061e41 --- /dev/null +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py @@ -0,0 +1,105 @@ +# Copyright 2021 Clearpath Robotics, Inc. +# @author Roni Kreinin (rkreinin@clearpathrobotics.com) + +import os + +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + + +class OffsetParser(Substitution): + def __init__( + self, + number: SomeSubstitutionsType, + offset: float, + ) -> None: + self.__number = number + self.__offset = offset + + def perform( + self, + context: LaunchContext = None, + ) -> str: + number = float(self.__number.perform(context)) + return f'{number + self.__offset}' + + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('world', default_value='depot', + description='Ignition World'), +] + +for pose_element in ['x', 'y', 'z', 'yaw']: + ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', + description=f'{pose_element} component of the robot pose.')) + + +def generate_launch_description(): + + # Directories + pkg_irobot_create_ignition_bringup = get_package_share_directory( + 'irobot_create_ignition_bringup') + pkg_irobot_create_ignition_plugins = get_package_share_directory( + 'irobot_create_ignition_plugins') + pkg_irobot_create_description = get_package_share_directory( + 'irobot_create_description') + pkg_ros_ign_gazebo = get_package_share_directory( + 'ros_ign_gazebo') + + # Set Ignition resource path + ign_resource_path = SetEnvironmentVariable(name='IGN_GAZEBO_RESOURCE_PATH', + value=[os.path.join( + pkg_irobot_create_ignition_bringup, + 'worlds'), ':' + + str(Path( + pkg_irobot_create_description). + parent.resolve())]) + + ign_gui_plugin_path = SetEnvironmentVariable(name='IGN_GUI_PLUGIN_PATH', + value=[os.path.join( + pkg_irobot_create_ignition_plugins, + 'lib')]) + + # Paths + ign_gazebo_launch = PathJoinSubstitution( + [pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py']) + + # Ignition gazebo + ignition_gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ign_gazebo_launch]), + launch_arguments=[ + ('ign_args', [LaunchConfiguration('world'), + '.sdf', + ' -v 4', + ' --gui-config ', + PathJoinSubstitution([pkg_irobot_create_ignition_bringup, + 'gui', 'create3', 'gui.config'])]) + ] + ) + + # clock bridge + clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' + ]) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(ign_resource_path) + ld.add_action(ign_gui_plugin_path) + ld.add_action(ignition_gazebo) + ld.add_action(clock_bridge) + return ld diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc index 94b506b7..42bcd15c 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc @@ -5,12 +5,12 @@ #include "Create3Hmi.hh" -#include +#include +#include +#include +#include -#include "ignition/gui/Application.hh" -#include "ignition/gui/MainWindow.hh" -#include "ignition/msgs/int32.pb.h" -#include "ignition/plugin/Register.hh" +#include namespace ignition { @@ -32,14 +32,17 @@ Create3Hmi::~Create3Hmi() void Create3Hmi::LoadConfig(const tinyxml2::XMLElement * _pluginElem) { - if (!_pluginElem) { - return; - } - if (this->title.empty()) { this->title = "Create3 HMI"; } + if (_pluginElem) + { + auto nameElem = _pluginElem->FirstChildElement("name"); + if (nullptr != nameElem && nullptr != nameElem->GetText()) + this->SetName(nameElem->GetText()); + } + this->connect( this, SIGNAL(AddMsg(QString)), this, SLOT(OnAddMsg(QString)), Qt::QueuedConnection); @@ -57,6 +60,38 @@ void Create3Hmi::OnCreate3Button(const int button) } } +QString Create3Hmi::Name() const +{ + return QString::fromStdString(this->robot_name_); +} + +void Create3Hmi::SetName(const QString &_name) +{ + this->robot_name_ = _name.toStdString(); + this->create3_button_topic_ = "/model/" + this->robot_name_ + "/create3_buttons"; + ignmsg << "A new robot name has been entered, publishing on topic: '" << + this->create3_button_topic_ << " ' " <create3_button_pub_ = gz::transport::Node::Publisher(); + this->create3_button_pub_ = + this->node_.Advertise< ignition::msgs::Int32 > + (this->create3_button_topic_); + if (!this->create3_button_pub_) + { + App()->findChild()->notifyWithDuration( + QString::fromStdString("Error when advertising topic: " + + this->create3_button_topic_), 4000); + ignerr << "Error when advertising topic: " << + this->create3_button_topic_ << std::endl; + }else { + App()->findChild()->notifyWithDuration( + QString::fromStdString("Advertising topic: '" + + this->create3_button_topic_ + "'"), 4000); + } + this->NameChanged(); +} + } // namespace gui } // namespace ignition @@ -64,4 +99,4 @@ void Create3Hmi::OnCreate3Button(const int button) // Register this plugin IGNITION_ADD_PLUGIN( ignition::gui::Create3Hmi, - ignition::gui::Plugin) + ignition::gui::Plugin) \ No newline at end of file diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh index e5b2a8a4..d9859c91 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh @@ -6,12 +6,12 @@ #ifndef IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ #define IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ +#include -#include +#include +#include -#include "ignition/gui/qt.h" -#include "ignition/gui/Plugin.hh" -#include "ignition/transport/Node.hh" +#include namespace ignition { @@ -23,6 +23,14 @@ class Create3Hmi : public Plugin { Q_OBJECT + // \brief Name + Q_PROPERTY( + QString name + READ Name + WRITE SetName + NOTIFY NameChanged + ) + public: /// \brief Constructor Create3Hmi(); @@ -31,18 +39,34 @@ public: /// \brief Called by Ignition GUI when plugin is instantiated. /// \param[in] _pluginElem XML configuration for this plugin. void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + // \brief Get the robot name as a string, for example + /// '/echo' + /// \return Name + Q_INVOKABLE QString Name() const; + +public slots: + /// \brief Callback in Qt thread when the robot name changes. + /// \param[in] _name variable to indicate the robot name to + /// publish the Button commands. + void SetName(const QString &_name); + +signals: + /// \brief Notify that robot name has changed + void NameChanged(); protected slots: /// \brief Callback trigged when the button is pressed. void OnCreate3Button(const int button); + private: ignition::transport::Node node_; ignition::transport::Node::Publisher create3_button_pub_; - std::string create3_button_topic_ = "/create3/buttons"; + std::string robot_name_ = "create3"; + std::string create3_button_topic_ = "/model/create3/create3_buttons"; }; } // namespace gui } // namespace ignition -#endif // IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ +#endif // IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ \ No newline at end of file diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml index cf03c0bd..cddb6674 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml @@ -17,30 +17,47 @@ Rectangle anchors.fill: parent focus: true Layout.minimumWidth: 400 - Layout.minimumHeight: 175 + Layout.minimumHeight: 225 Rectangle { id: create3ButtonsRectangle - border.color: "black" border.width: 2 anchors.top: widgetRectangle.top anchors.left: widgetRectangle.left focus: true - height: 125 + height: 175 width: 400 - // Buttons + // Robot Name input Label { - id: create3ButtonsLabel - text: "Create3" - font.pixelSize: 22 + id: nameLabel + text: "Robot Name:" + Layout.fillWidth: true + Layout.margins: 10 anchors.top: create3ButtonsRectangle.top anchors.topMargin: 10 - anchors.left: parent.left + anchors.left: create3ButtonsRectangle.left anchors.leftMargin: 10 } + TextField { + id: nameField + width: 175 + Layout.fillWidth: true + Layout.margins: 10 + text: Create3Hmi.name + placeholderText: qsTr("input robot name") + anchors.top: nameLabel.bottom + anchors.topMargin: 5 + anchors.left: create3ButtonsRectangle.left + anchors.leftMargin: 10 + onEditingFinished: { + Create3Hmi.SetName(text) + } + } + + // Button inputs ToolButton { id: create3Button1 anchors.verticalCenter: create3ButtonPower.verticalCenter @@ -62,8 +79,8 @@ Rectangle ToolButton { id: create3ButtonPower - anchors.top: create3ButtonsLabel.bottom - anchors.topMargin: 0 + anchors.bottom: create3ButtonsRectangle.bottom + anchors.bottomMargin: 15 anchors.horizontalCenter: create3ButtonsRectangle.horizontalCenter checkable: true checked: true @@ -99,4 +116,3 @@ Rectangle } } } - diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp index f5b751ca..93821f1e 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp @@ -12,7 +12,7 @@ InterfaceButtons::InterfaceButtons() : rclcpp::Node("sensors_node") { interface_buttons_sub_ = this->create_subscription( - "create3_buttons", + "_create3_buttons", rclcpp::SensorDataQoS(), std::bind(&InterfaceButtons::create3_buttons_callback, this, std::placeholders::_1)); From 5b69b229d5614b6b09fa0212f521014ea51a800c Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Fri, 10 Mar 2023 13:02:51 -0500 Subject: [PATCH 03/14] Dock model is now called /standard_dock Namespaced dock frame id in pose republisher --- .../launch/create3_ros_ignition_bridge.launch.py | 6 +++--- .../launch/create3_spawn.launch.py | 2 +- .../pose_republisher/pose_republisher.hpp | 1 + .../src/pose_republisher/pose_republisher_node.cpp | 7 ++++++- 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py index 9bbf1fba..3b860fd3 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py @@ -71,14 +71,14 @@ def generate_launch_description(): ['/model/', robot_name, '/pose' + '@tf2_msgs/msg/TFMessage' + '[ignition.msgs.Pose_V'], - '/model/standard_dock/pose' + + ['/model/', robot_name, '/standard_dock/pose' + '@tf2_msgs/msg/TFMessage' + - '[ignition.msgs.Pose_V' + '[ignition.msgs.Pose_V'] ], remappings=[ (['/model/', robot_name, '/pose'], '_internal/sim_ground_truth_pose'), - ('/model/standard_dock/pose', + (['/model/', robot_name, '/standard_dock/pose'], '_internal/sim_ground_truth_dock_pose') ]) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py index 98e7eab7..3113106e 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py @@ -120,7 +120,7 @@ def generate_launch_description(): Node( package='ros_ign_gazebo', executable='create', - arguments=['-name', [LaunchConfiguration('robot_name'), '_standard_dock'], + arguments=['-name', [LaunchConfiguration('robot_name'), '/standard_dock'], '-x', x_dock, '-y', y, '-z', z, diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp b/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp index 18dd19eb..d7171fa2 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp @@ -47,6 +47,7 @@ class PoseRepublisher : public rclcpp::Node std::string robot_name_; std::string wheel_joints_[2]; + std::string standard_dock_frame_id_; }; } // namespace irobot_create_ignition_toolbox diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp index a4601eaa..34832641 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp @@ -71,6 +71,11 @@ PoseRepublisher::PoseRepublisher() dynamic_joint_state_publisher_ = create_publisher( "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); + + // Standard dock frame id is namespaced + standard_dock_frame_id_ = get_effective_namespace() + "/standard_dock"; + // Remove leading '/' + standard_dock_frame_id_.erase(0, 1); } void PoseRepublisher::robot_subscriber_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) @@ -116,7 +121,7 @@ void PoseRepublisher::dock_subscriber_callback(const tf2_msgs::msg::TFMessage::S { for (uint16_t i = 0; i < msg->transforms.size(); i++) { // Child frame is model name - if (msg->transforms[i].child_frame_id == "standard_dock") { + if (msg->transforms[i].child_frame_id == standard_dock_frame_id_) { auto odom_msg = utils::tf_message_to_odom(msg, i); // Save dock pose tf2::convert(odom_msg->pose.pose, last_dock_pose_); From bc5df60024f83807dca98c729f14331fcb9e38be Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Fri, 10 Mar 2023 16:02:17 -0500 Subject: [PATCH 04/14] Added namespacing support to Rviz config Rviz now launches for each robot spawn Added static transform for prefixed odom and base_link frames Added offset launch module in irobot_create_common_bringup Fixed spawning robots with different x, y, and yaw positions --- .../CMakeLists.txt | 4 ++ .../irobot_create_common_bringup/__init__.py | 0 .../irobot_create_common_bringup/offset.py | 59 +++++++++++++++++++ .../launch/dock_description.launch.py | 19 ++---- .../launch/rviz2.launch.py | 4 ++ .../rviz/irobot_create_view.rviz | 10 ++-- .../launch/include/control.py | 37 +++++++++++- .../launch/create3_ignition.launch.py | 17 +----- .../launch/create3_spawn.launch.py | 51 ++++++++-------- .../launch/ignition.launch.py | 17 ------ .../Create3Hmi/Create3Hmi.cc | 2 +- .../pose_republisher_node.cpp | 1 + 12 files changed, 141 insertions(+), 80 deletions(-) create mode 100644 irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/__init__.py create mode 100644 irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/offset.py diff --git a/irobot_create_common/irobot_create_common_bringup/CMakeLists.txt b/irobot_create_common/irobot_create_common_bringup/CMakeLists.txt index caa2b70e..cc7942f5 100644 --- a/irobot_create_common/irobot_create_common_bringup/CMakeLists.txt +++ b/irobot_create_common/irobot_create_common_bringup/CMakeLists.txt @@ -11,6 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) install( DIRECTORY @@ -21,6 +22,9 @@ install( share/${PROJECT_NAME} ) +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/__init__.py b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/offset.py b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/offset.py new file mode 100644 index 00000000..033b3c11 --- /dev/null +++ b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/offset.py @@ -0,0 +1,59 @@ +import math +from typing import Union + +from launch import LaunchContext, SomeSubstitutionsType, Substitution + + +class RotationalOffsetX(Substitution): + def __init__( + self, + offset: float, + yaw: SomeSubstitutionsType + ) -> None: + self.__offset = offset + self.__yaw = yaw + + def perform( + self, + context: LaunchContext = None, + ) -> str: + yaw = float(self.__yaw.perform(context)) + return f'{self.__offset * math.cos(yaw)}' + + +class RotationalOffsetY(Substitution): + def __init__( + self, + offset: float, + yaw: SomeSubstitutionsType + ) -> None: + self.__offset = offset + self.__yaw = yaw + + def perform( + self, + context: LaunchContext = None, + ) -> str: + yaw = float(self.__yaw.perform(context)) + return f'{self.__offset * math.sin(yaw)}' + + +class OffsetParser(Substitution): + def __init__( + self, + number: SomeSubstitutionsType, + offset: Union[float, Substitution], + ) -> None: + self.__number = number + self.__offset = offset + + def perform( + self, + context: LaunchContext = None, + ) -> str: + number = float(self.__number.perform(context)) + if isinstance(self.__offset, Substitution): + offset = float(self.__offset.perform(context)) + else: + offset = self.__offset + return f'{number + offset}' diff --git a/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py index 4a65fe71..f5781fcb 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py @@ -9,19 +9,14 @@ from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node - ARGUMENTS = [ DeclareLaunchArgument('gazebo', default_value='classic', choices=['classic', 'ignition'], - description='Which gazebo simulation to use') -] -for pose_element in ['x', 'y', 'z', 'yaw']: - ARGUMENTS.append(DeclareLaunchArgument(f'{pose_element}', default_value='0.0', - description=f'{pose_element} component of the dock pose.')) - -ARGUMENTS.append(DeclareLaunchArgument('visualize_rays', default_value='true', + description='Which gazebo simulation to use'), + DeclareLaunchArgument('visualize_rays', default_value='true', choices=['true', 'false'], - description='Enable/disable ray visualization')) + description='Enable/disable ray visualization') +] def generate_launch_description(): @@ -32,8 +27,6 @@ def generate_launch_description(): [pkg_create3_description, 'urdf', 'dock', 'standard_dock.urdf.xacro']) # Launch Configurations - x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') - yaw = LaunchConfiguration('yaw') visualize_rays = LaunchConfiguration('visualize_rays') gazebo_simulator = LaunchConfiguration('gazebo') @@ -62,10 +55,10 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', name='tf_odom_std_dock_link_publisher', - arguments=[x, y, z, + arguments=['0.157', '0', '0', # According to documentation (http://wiki.ros.org/tf2_ros): # the order is yaw, pitch, roll - yaw, '0', '0', + '3.141592', '0', '0', 'odom', 'std_dock_link'], remappings=[ ('/tf', 'tf'), diff --git a/irobot_create_common/irobot_create_common_bringup/launch/rviz2.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/rviz2.launch.py index 2fea187d..f5fd3798 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/rviz2.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/rviz2.launch.py @@ -23,6 +23,10 @@ def generate_launch_description(): arguments=[ '--display-config', rviz_config, '--splash-screen', rviz_logo, + ], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), ] ) diff --git a/irobot_create_common/irobot_create_common_bringup/rviz/irobot_create_view.rviz b/irobot_create_common/irobot_create_common_bringup/rviz/irobot_create_view.rviz index 8b7fe939..c44b6499 100644 --- a/irobot_create_common/irobot_create_common_bringup/rviz/irobot_create_view.rviz +++ b/irobot_create_common/irobot_create_common_bringup/rviz/irobot_create_view.rviz @@ -52,7 +52,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: robot_description Enabled: true Links: All Links Enabled: true @@ -116,7 +116,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /standard_dock_description + Value: standard_dock_description Enabled: true Links: All Links Enabled: true @@ -184,14 +184,14 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /initialpose + Value: initialpose - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /goal_pose + Value: goal_pose - Class: rviz_default_plugins/PublishPoint Single click: true Topic: @@ -199,7 +199,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /clicked_point + Value: clicked_point Transformation: Current: Class: rviz_default_plugins/TF diff --git a/irobot_create_common/irobot_create_control/launch/include/control.py b/irobot_create_common/irobot_create_control/launch/include/control.py index 4488f00b..e889e6d9 100644 --- a/irobot_create_common/irobot_create_control/launch/include/control.py +++ b/irobot_create_common/irobot_create_control/launch/include/control.py @@ -29,14 +29,14 @@ def generate_launch_description(): executable='spawner', namespace=robot_name, # Namespace is not pushed when used in EventHandler parameters=[control_params_file], - arguments=['diffdrive_controller', '-c', 'controller_manager', '-n', robot_name], + arguments=['diffdrive_controller', '-c', 'controller_manager'], output='screen', ) joint_state_broadcaster_spawner = Node( package='controller_manager', executable='spawner', - arguments=['joint_state_broadcaster', '-c', 'controller_manager', '-n', robot_name], + arguments=['joint_state_broadcaster', '-c', 'controller_manager'], output='screen', ) @@ -48,9 +48,42 @@ def generate_launch_description(): ) ) + # Static transform from /odom to odom + # See https://github.com/ros-controls/ros2_controllers/pull/533 + tf_namespaced_odom_publisher = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='tf_namespaced_odom_publisher', + arguments=['0', '0', '0', + '0', '0', '0', + [LaunchConfiguration('robot_name'), '/odom'], 'odom'], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ], + output='screen', + ) + + # Static transform from /base_link to base_link + tf_namespaced_base_link_publisher = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='tf_namespaced_base_link_publisher', + arguments=['0', '0', '0', + '0', '0', '0', + [LaunchConfiguration('robot_name'), '/base_link'], 'base_link'], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ], + output='screen', + ) + ld = LaunchDescription(ARGUMENTS) ld.add_action(joint_state_broadcaster_spawner) ld.add_action(diffdrive_controller_callback) + ld.add_action(tf_namespaced_odom_publisher) + ld.add_action(tf_namespaced_base_link_publisher) return ld diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py index b0c1da5c..7552e3a6 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py @@ -6,7 +6,6 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription -from launch.conditions import IfCondition, LaunchConfigurationNotEquals from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution @@ -14,7 +13,7 @@ ARGUMENTS = [ DeclareLaunchArgument('robot_name', default_value='create3', description='Robot name'), - DeclareLaunchArgument('use_rviz', default_value='false', + DeclareLaunchArgument('use_rviz', default_value='true', choices=['true', 'false'], description='Start rviz.'), DeclareLaunchArgument('world', default_value='depot', description='Ignition World'), @@ -30,14 +29,10 @@ def generate_launch_description(): # Directories pkg_irobot_create_ignition_bringup = get_package_share_directory( 'irobot_create_ignition_bringup') - pkg_irobot_create_common_bringup = get_package_share_directory( - 'irobot_create_common_bringup') # Paths ignition_launch = PathJoinSubstitution( [pkg_irobot_create_ignition_bringup, 'launch', 'ignition.launch.py']) - rviz2_launch = PathJoinSubstitution( - [pkg_irobot_create_common_bringup, 'launch', 'rviz2.launch.py']) robot_spawn_launch = PathJoinSubstitution( [pkg_irobot_create_ignition_bringup, 'launch', 'create3_spawn.launch.py']) @@ -48,24 +43,18 @@ def generate_launch_description(): ] ) - rviz2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource([rviz2_launch]), - condition=IfCondition(LaunchConfiguration('use_rviz')), - ) - robot_spawn = IncludeLaunchDescription( PythonLaunchDescriptionSource([robot_spawn_launch]), launch_arguments=[ ('robot_name', LaunchConfiguration('robot_name')), + ('use_rviz', LaunchConfiguration('use_rviz')), ('x', LaunchConfiguration('x')), ('y', LaunchConfiguration('y')), ('z', LaunchConfiguration('z')), - ('yaw', LaunchConfiguration('yaw'))], - condition=LaunchConfigurationNotEquals('robot_name', '')) + ('yaw', LaunchConfiguration('yaw'))]) # Create launch description and add actions ld = LaunchDescription(ARGUMENTS) ld.add_action(ignition) - ld.add_action(rviz2) ld.add_action(robot_spawn) return ld diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py index 3113106e..ca5a175c 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py @@ -1,13 +1,11 @@ # Copyright 2021 Clearpath Robotics, Inc. # @author Roni Kreinin (rkreinin@clearpathrobotics.com) -import os - -from pathlib import Path - from ament_index_python.packages import get_package_share_directory -from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution +from irobot_create_common_bringup.offset import OffsetParser, RotationalOffsetX, RotationalOffsetY + +from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction from launch.actions import IncludeLaunchDescription from launch.conditions import IfCondition @@ -16,23 +14,6 @@ from launch_ros.actions import Node, PushRosNamespace -class OffsetParser(Substitution): - def __init__( - self, - number: SomeSubstitutionsType, - offset: float, - ) -> None: - self.__number = number - self.__offset = offset - - def perform( - self, - context: LaunchContext = None, - ) -> str: - number = float(self.__number.perform(context)) - return f'{number + self.__offset}' - - ARGUMENTS = [ DeclareLaunchArgument('bridge', default_value='true', choices=['true', 'false'], @@ -75,13 +56,21 @@ def generate_launch_description(): [pkg_irobot_create_common_bringup, 'launch', 'robot_description.launch.py']) dock_description_launch = PathJoinSubstitution( [pkg_irobot_create_common_bringup, 'launch', 'dock_description.launch.py']) + rviz2_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'rviz2.launch.py']) # Launch configurations namespace = LaunchConfiguration('robot_name') x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') yaw = LaunchConfiguration('yaw') - x_dock = OffsetParser(x, 0.157) + # Calculate dock offset due to yaw rotation + dock_offset_x = RotationalOffsetX(0.157, yaw) + dock_offset_y = RotationalOffsetY(0.157, yaw) + # Spawn dock at robot position + rotational offset + x_dock = OffsetParser(x, dock_offset_x) + y_dock = OffsetParser(y, dock_offset_y) + # Rotate dock towards robot yaw_dock = OffsetParser(yaw, 3.1416) spawn_robot_group_action = GroupAction([ @@ -92,8 +81,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource([dock_description_launch]), condition=IfCondition(LaunchConfiguration('spawn_dock')), # The robot starts docked - launch_arguments={'x': x_dock, 'y': y, 'z': z, 'yaw': yaw_dock, - 'gazebo': 'ignition'}.items(), + launch_arguments={'gazebo': 'ignition'}.items(), ), # Robot description @@ -111,7 +99,7 @@ def generate_launch_description(): '-x', x, '-y', y, '-z', z, - '-Y', '0.0', + '-Y', yaw, '-topic', 'robot_description'], output='screen' ), @@ -122,11 +110,12 @@ def generate_launch_description(): executable='create', arguments=['-name', [LaunchConfiguration('robot_name'), '/standard_dock'], '-x', x_dock, - '-y', y, + '-y', y_dock, '-z', z, - '-Y', '3.141592', + '-Y', yaw_dock, '-topic', 'standard_dock_description'], output='screen', + condition=IfCondition(LaunchConfiguration('spawn_dock')) ), # ROS Ign Bridge @@ -146,6 +135,12 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), launch_arguments=[('robot_name', LaunchConfiguration('robot_name'))] + ), + + # Rviz + IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz2_launch]), + condition=IfCondition(LaunchConfiguration('use_rviz')), ) ]) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py index fd061e41..c4415cba 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py @@ -15,23 +15,6 @@ from launch_ros.actions import Node -class OffsetParser(Substitution): - def __init__( - self, - number: SomeSubstitutionsType, - offset: float, - ) -> None: - self.__number = number - self.__offset = offset - - def perform( - self, - context: LaunchContext = None, - ) -> str: - number = float(self.__number.perform(context)) - return f'{number + self.__offset}' - - ARGUMENTS = [ DeclareLaunchArgument('use_sim_time', default_value='true', choices=['true', 'false'], diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc index 42bcd15c..4d7aed3b 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc @@ -73,7 +73,7 @@ void Create3Hmi::SetName(const QString &_name) this->create3_button_topic_ << " ' " <create3_button_pub_ = gz::transport::Node::Publisher(); + this->create3_button_pub_ = ignition::transport::Node::Publisher(); this->create3_button_pub_ = this->node_.Advertise< ignition::msgs::Int32 > (this->create3_button_topic_); diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp index 34832641..efac1a04 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp @@ -119,6 +119,7 @@ void PoseRepublisher::robot_subscriber_callback(const tf2_msgs::msg::TFMessage:: void PoseRepublisher::dock_subscriber_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { + for (uint16_t i = 0; i < msg->transforms.size(); i++) { // Child frame is model name if (msg->transforms[i].child_frame_id == standard_dock_frame_id_) { From e87bd6b098ba491076492aee834fde6d0e382a41 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Fri, 10 Mar 2023 16:27:03 -0500 Subject: [PATCH 05/14] Linter fixes --- .../launch/dock_description.launch.py | 4 ++-- .../irobot_create_control/launch/include/control.py | 2 +- .../launch/create3_ros_ignition_bridge.launch.py | 12 ++++++------ .../launch/ignition.launch.py | 6 +----- .../Create3Hmi/Create3Hmi.cc | 8 +++++--- .../Create3Hmi/Create3Hmi.hh | 9 +++++---- .../src/pose_republisher/pose_republisher_node.cpp | 3 +-- 7 files changed, 21 insertions(+), 23 deletions(-) diff --git a/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py index f5781fcb..0c632055 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py @@ -14,8 +14,8 @@ choices=['classic', 'ignition'], description='Which gazebo simulation to use'), DeclareLaunchArgument('visualize_rays', default_value='true', - choices=['true', 'false'], - description='Enable/disable ray visualization') + choices=['true', 'false'], + description='Enable/disable ray visualization') ] diff --git a/irobot_create_common/irobot_create_control/launch/include/control.py b/irobot_create_common/irobot_create_control/launch/include/control.py index e889e6d9..bc36782c 100644 --- a/irobot_create_common/irobot_create_control/launch/include/control.py +++ b/irobot_create_common/irobot_create_control/launch/include/control.py @@ -27,7 +27,7 @@ def generate_launch_description(): diffdrive_controller_node = Node( package='controller_manager', executable='spawner', - namespace=robot_name, # Namespace is not pushed when used in EventHandler + namespace=robot_name, # Namespace is not pushed when used in EventHandler parameters=[control_params_file], arguments=['diffdrive_controller', '-c', 'controller_manager'], output='screen', diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py index 3b860fd3..fea8d497 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py @@ -72,8 +72,8 @@ def generate_launch_description(): '@tf2_msgs/msg/TFMessage' + '[ignition.msgs.Pose_V'], ['/model/', robot_name, '/standard_dock/pose' + - '@tf2_msgs/msg/TFMessage' + - '[ignition.msgs.Pose_V'] + '@tf2_msgs/msg/TFMessage' + + '[ignition.msgs.Pose_V'] ], remappings=[ (['/model/', robot_name, '/pose'], @@ -149,15 +149,15 @@ def generate_launch_description(): }], arguments=[ ['/world/', world, - '/model/', robot_name, - '/link/' + ir + '/sensor/' + ir + '/scan' + - '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + '/model/', robot_name, + '/link/' + ir + '/sensor/' + ir + '/scan' + + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] ], remappings=[ (['/world/', world, '/model/', robot_name, '/link/' + ir + '/sensor/' + ir + '/scan'], - '_internal/' + ir + '/scan') + '_internal/' + ir + '/scan') ]) for ir in ir_intensity_sensors ]) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py index c4415cba..835cc937 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py @@ -7,7 +7,7 @@ from ament_index_python.packages import get_package_share_directory -from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution +from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -23,10 +23,6 @@ description='Ignition World'), ] -for pose_element in ['x', 'y', 'z', 'yaw']: - ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', - description=f'{pose_element} component of the robot pose.')) - def generate_launch_description(): diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc index 4d7aed3b..b48146c9 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc @@ -5,12 +5,14 @@ #include "Create3Hmi.hh" +#include + +#include + #include #include #include -#include -#include namespace ignition { @@ -99,4 +101,4 @@ void Create3Hmi::SetName(const QString &_name) // Register this plugin IGNITION_ADD_PLUGIN( ignition::gui::Create3Hmi, - ignition::gui::Plugin) \ No newline at end of file + ignition::gui::Plugin) diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh index d9859c91..0747fb53 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh @@ -6,13 +6,14 @@ #ifndef IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ #define IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ -#include - #include -#include #include +#include +#include + + namespace ignition { @@ -69,4 +70,4 @@ private: } // namespace ignition -#endif // IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ \ No newline at end of file +#endif // IROBOT_CREATE_IGNITION__IROBOT_CREATE_IGNITION_PLUGINS__CREATE3HMI__CREATE3HMI_HH_ diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp index efac1a04..7659cb53 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp @@ -75,7 +75,7 @@ PoseRepublisher::PoseRepublisher() // Standard dock frame id is namespaced standard_dock_frame_id_ = get_effective_namespace() + "/standard_dock"; // Remove leading '/' - standard_dock_frame_id_.erase(0, 1); + standard_dock_frame_id_.erase(0, 1); } void PoseRepublisher::robot_subscriber_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) @@ -119,7 +119,6 @@ void PoseRepublisher::robot_subscriber_callback(const tf2_msgs::msg::TFMessage:: void PoseRepublisher::dock_subscriber_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { - for (uint16_t i = 0; i < msg->transforms.size(); i++) { // Child frame is model name if (msg->transforms[i].child_frame_id == standard_dock_frame_id_) { From 872a633bbf430cb31823741aee7780e14a87ba4a Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Mon, 13 Mar 2023 13:15:51 -0400 Subject: [PATCH 06/14] Added namespace launch arg. Defaults to 'robot_name' in create3_spawn create3_ignition defaults to spawning 'create3' robot without namespace --- .../launch/robot_description.launch.py | 6 +- .../launch/include/control.py | 14 ++-- .../urdf/create3.urdf.xacro | 2 +- .../gui/create3/gui.config | 2 +- .../launch/create3_ignition.launch.py | 3 + .../create3_ros_ignition_bridge.launch.py | 78 ++++++++++--------- .../launch/create3_spawn.launch.py | 16 ++-- .../Create3Hmi/Create3Hmi.cc | 19 ++--- .../Create3Hmi/Create3Hmi.hh | 16 ++-- .../Create3Hmi/Create3Hmi.qml | 14 ++-- .../interface_buttons_node.cpp | 2 +- 11 files changed, 93 insertions(+), 79 deletions(-) diff --git a/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py index 80f2bfd2..48b15e49 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py @@ -20,6 +20,8 @@ description='Enable/disable ray visualization'), DeclareLaunchArgument('robot_name', default_value='create3', description='Robot name'), + DeclareLaunchArgument('namespace', default_value=LaunchConfiguration('robot_name'), + description='Robot namespace'), ] @@ -28,7 +30,7 @@ def generate_launch_description(): xacro_file = PathJoinSubstitution([pkg_create3_description, 'urdf', 'create3.urdf.xacro']) gazebo_simulator = LaunchConfiguration('gazebo') visualize_rays = LaunchConfiguration('visualize_rays') - robot_name = LaunchConfiguration('robot_name') + namespace = LaunchConfiguration('namespace') robot_state_publisher = Node( package='robot_state_publisher', @@ -42,7 +44,7 @@ def generate_launch_description(): ['xacro', ' ', xacro_file, ' ', 'gazebo:=', gazebo_simulator, ' ', 'visualize_rays:=', visualize_rays, ' ', - 'namespace:=', robot_name])}, + 'namespace:=', namespace])}, ], remappings=[ ('/tf', 'tf'), diff --git a/irobot_create_common/irobot_create_control/launch/include/control.py b/irobot_create_common/irobot_create_control/launch/include/control.py index bc36782c..a07763c7 100644 --- a/irobot_create_common/irobot_create_control/launch/include/control.py +++ b/irobot_create_common/irobot_create_control/launch/include/control.py @@ -13,13 +13,15 @@ ARGUMENTS = [ DeclareLaunchArgument('robot_name', default_value='create3', description='Robot name'), + DeclareLaunchArgument('namespace', default_value=LaunchConfiguration('robot_name'), + description='Robot namespace'), ] def generate_launch_description(): pkg_create3_control = get_package_share_directory('irobot_create_control') - robot_name = LaunchConfiguration('robot_name') + namespace = LaunchConfiguration('namespace') control_params_file = PathJoinSubstitution( [pkg_create3_control, 'config', 'control.yaml']) @@ -27,7 +29,7 @@ def generate_launch_description(): diffdrive_controller_node = Node( package='controller_manager', executable='spawner', - namespace=robot_name, # Namespace is not pushed when used in EventHandler + namespace=namespace, # Namespace is not pushed when used in EventHandler parameters=[control_params_file], arguments=['diffdrive_controller', '-c', 'controller_manager'], output='screen', @@ -48,7 +50,7 @@ def generate_launch_description(): ) ) - # Static transform from /odom to odom + # Static transform from /odom to odom # See https://github.com/ros-controls/ros2_controllers/pull/533 tf_namespaced_odom_publisher = Node( package='tf2_ros', @@ -56,7 +58,7 @@ def generate_launch_description(): name='tf_namespaced_odom_publisher', arguments=['0', '0', '0', '0', '0', '0', - [LaunchConfiguration('robot_name'), '/odom'], 'odom'], + [LaunchConfiguration('namespace'), '/odom'], 'odom'], remappings=[ ('/tf', 'tf'), ('/tf_static', 'tf_static') @@ -64,14 +66,14 @@ def generate_launch_description(): output='screen', ) - # Static transform from /base_link to base_link + # Static transform from /base_link to base_link tf_namespaced_base_link_publisher = Node( package='tf2_ros', executable='static_transform_publisher', name='tf_namespaced_base_link_publisher', arguments=['0', '0', '0', '0', '0', '0', - [LaunchConfiguration('robot_name'), '/base_link'], 'base_link'], + [LaunchConfiguration('namespace'), '/base_link'], 'base_link'], remappings=[ ('/tf', 'tf'), ('/tf_static', 'tf_static') diff --git a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro index 0eeabd4f..e82d117a 100644 --- a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro @@ -15,7 +15,7 @@ - + diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config b/irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config index 626f4649..3f512c9b 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config +++ b/irobot_create_ignition/irobot_create_ignition_bringup/gui/create3/gui.config @@ -103,7 +103,7 @@ - /create3/cmd_vel + /cmd_vel true docked diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py index 7552e3a6..9ec75bde 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py @@ -13,6 +13,8 @@ ARGUMENTS = [ DeclareLaunchArgument('robot_name', default_value='create3', description='Robot name'), + DeclareLaunchArgument('namespace', default_value='', + description='Robot namespace'), DeclareLaunchArgument('use_rviz', default_value='true', choices=['true', 'false'], description='Start rviz.'), DeclareLaunchArgument('world', default_value='depot', @@ -47,6 +49,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource([robot_spawn_launch]), launch_arguments=[ ('robot_name', LaunchConfiguration('robot_name')), + ('namespace', LaunchConfiguration('namespace')), ('use_rviz', LaunchConfiguration('use_rviz')), ('x', LaunchConfiguration('x')), ('y', LaunchConfiguration('y')), diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py index fea8d497..a85de569 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py @@ -13,6 +13,8 @@ description='Use sim time'), DeclareLaunchArgument('robot_name', default_value='create3', description='Ignition model name'), + DeclareLaunchArgument('namespace', default_value=LaunchConfiguration('robot_name'), + description='Robot namespace'), DeclareLaunchArgument('world', default_value='depot', description='World name') ] @@ -21,6 +23,7 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') robot_name = LaunchConfiguration('robot_name') + namespace = LaunchConfiguration('namespace') world = LaunchConfiguration('world') cliff_sensors = [ @@ -41,24 +44,26 @@ def generate_launch_description(): ] # cmd_vel bridge - cmd_vel_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', - name='cmd_vel_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[ - [robot_name, - '/cmd_vel' + '@geometry_msgs/msg/Twist' + '[ignition.msgs.Twist'], - ['/model/', robot_name, '/cmd_vel' + - '@geometry_msgs/msg/Twist' + - ']ignition.msgs.Twist'] - ], - remappings=[ - ([robot_name, '/cmd_vel'], 'cmd_vel'), - (['/model/', robot_name, '/cmd_vel'], - 'diffdrive_controller/cmd_vel_unstamped') - ]) + cmd_vel_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='cmd_vel_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[ + [namespace, + '/cmd_vel' + '@geometry_msgs/msg/Twist' + '[ignition.msgs.Twist'], + ['/model/', robot_name, '/cmd_vel' + + '@geometry_msgs/msg/Twist' + + ']ignition.msgs.Twist'] + ], + remappings=[ + ([namespace, '/cmd_vel'], 'cmd_vel'), + (['/model/', robot_name, '/cmd_vel'], + 'diffdrive_controller/cmd_vel_unstamped') + ]) # Pose bridge pose_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', @@ -127,9 +132,9 @@ def generate_launch_description(): }], arguments=[ ['/world/', world, - '/model/', robot_name, - '/link/base_link/sensor/' + cliff + '/scan' + - '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + '/model/', robot_name, + '/link/base_link/sensor/' + cliff + '/scan' + + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] ], remappings=[ (['/world/', world, @@ -162,20 +167,23 @@ def generate_launch_description(): ]) # Buttons message bridge - buttons_msg_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', - name='buttons_msg_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[ - ['/model/', robot_name, '/create3_buttons' + - '@std_msgs/msg/Int32' + - '[ignition.msgs.Int32'] - ], - remappings=[ - (['/model/', robot_name, '/create3_buttons'], '_create3_buttons') - ]) + buttons_msg_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='buttons_msg_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[ + [namespace, '/create3_buttons' + + '@std_msgs/msg/Int32' + + '[ignition.msgs.Int32'], + ], + remappings=[ + ([namespace, '/create3_buttons'], '_internal/create3_buttons'), + ] + ) # Create launch description and add actions ld = LaunchDescription(ARGUMENTS) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py index ca5a175c..427e547c 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py @@ -25,6 +25,8 @@ description='Ignition World'), DeclareLaunchArgument('robot_name', default_value='create3', description='Robot name'), + DeclareLaunchArgument('namespace', default_value=LaunchConfiguration('robot_name'), + description='Robot namespace'), DeclareLaunchArgument('use_rviz', default_value='true', choices=['true', 'false'], description='Start rviz.'), DeclareLaunchArgument('spawn_dock', default_value='true', @@ -60,7 +62,7 @@ def generate_launch_description(): [pkg_irobot_create_common_bringup, 'launch', 'rviz2.launch.py']) # Launch configurations - namespace = LaunchConfiguration('robot_name') + namespace = LaunchConfiguration('namespace') x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') yaw = LaunchConfiguration('yaw') @@ -87,8 +89,7 @@ def generate_launch_description(): # Robot description IncludeLaunchDescription( PythonLaunchDescriptionSource([robot_description_launch]), - launch_arguments={'gazebo': 'ignition', - 'robot_name': LaunchConfiguration('robot_name')}.items() + launch_arguments={'gazebo': 'ignition'}.items() ), # Spawn Create 3 @@ -121,20 +122,17 @@ def generate_launch_description(): # ROS Ign Bridge IncludeLaunchDescription( PythonLaunchDescriptionSource([ros_ign_bridge_launch]), - launch_arguments=[('world', LaunchConfiguration('world')), - ('robot_name', LaunchConfiguration('robot_name'))] + launch_arguments=[('world', LaunchConfiguration('world'))] ), # Create 3 nodes IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_nodes_launch]), - launch_arguments=[('robot_name', LaunchConfiguration('robot_name'))] + PythonLaunchDescriptionSource([create3_nodes_launch]) ), # Create 3 Ignition nodes IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), - launch_arguments=[('robot_name', LaunchConfiguration('robot_name'))] + PythonLaunchDescriptionSource([create3_ignition_nodes_launch]) ), # Rviz diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc index b48146c9..cae270e5 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc @@ -40,9 +40,9 @@ void Create3Hmi::LoadConfig(const tinyxml2::XMLElement * _pluginElem) if (_pluginElem) { - auto nameElem = _pluginElem->FirstChildElement("name"); - if (nullptr != nameElem && nullptr != nameElem->GetText()) - this->SetName(nameElem->GetText()); + auto namespaceElem = _pluginElem->FirstChildElement("namespace"); + if (nullptr != namespaceElem && nullptr != namespaceElem->GetText()) + this->SetNamespace(namespaceElem->GetText()); } this->connect( @@ -62,15 +62,16 @@ void Create3Hmi::OnCreate3Button(const int button) } } -QString Create3Hmi::Name() const +QString Create3Hmi::Namespace() const { - return QString::fromStdString(this->robot_name_); + return QString::fromStdString(this->namespace_); } -void Create3Hmi::SetName(const QString &_name) +void Create3Hmi::SetNamespace(const QString &_name) { - this->robot_name_ = _name.toStdString(); - this->create3_button_topic_ = "/model/" + this->robot_name_ + "/create3_buttons"; + this->namespace_ = _name.toStdString(); + this->create3_button_topic_ = this->namespace_ + "/create3_buttons"; + ignmsg << "A new robot name has been entered, publishing on topic: '" << this->create3_button_topic_ << " ' " <" + this->create3_button_topic_ + "'"), 4000); } - this->NameChanged(); + this->NamespaceChanged(); } } // namespace gui diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh index 0747fb53..e8dd0dc0 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh @@ -27,9 +27,9 @@ class Create3Hmi : public Plugin // \brief Name Q_PROPERTY( QString name - READ Name - WRITE SetName - NOTIFY NameChanged + READ Namespace + WRITE SetNamespace + NOTIFY NamespaceChanged ) public: @@ -43,17 +43,17 @@ public: // \brief Get the robot name as a string, for example /// '/echo' /// \return Name - Q_INVOKABLE QString Name() const; + Q_INVOKABLE QString Namespace() const; public slots: /// \brief Callback in Qt thread when the robot name changes. /// \param[in] _name variable to indicate the robot name to /// publish the Button commands. - void SetName(const QString &_name); + void SetNamespace(const QString &_name); signals: /// \brief Notify that robot name has changed - void NameChanged(); + void NamespaceChanged(); protected slots: /// \brief Callback trigged when the button is pressed. @@ -62,8 +62,8 @@ protected slots: private: ignition::transport::Node node_; ignition::transport::Node::Publisher create3_button_pub_; - std::string robot_name_ = "create3"; - std::string create3_button_topic_ = "/model/create3/create3_buttons"; + std::string namespace_ = ""; + std::string create3_button_topic_ = "/create3_buttons"; }; } // namespace gui diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml index cddb6674..b2dcde64 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml @@ -29,10 +29,10 @@ Rectangle height: 175 width: 400 - // Robot Name input + // Robot namespace input Label { - id: nameLabel - text: "Robot Name:" + id: namespaceLabel + text: "Namespace:" Layout.fillWidth: true Layout.margins: 10 anchors.top: create3ButtonsRectangle.top @@ -46,14 +46,14 @@ Rectangle width: 175 Layout.fillWidth: true Layout.margins: 10 - text: Create3Hmi.name - placeholderText: qsTr("input robot name") - anchors.top: nameLabel.bottom + text: Create3Hmi.namespace + placeholderText: qsTr("Robot namespace") + anchors.top: namespaceLabel.bottom anchors.topMargin: 5 anchors.left: create3ButtonsRectangle.left anchors.leftMargin: 10 onEditingFinished: { - Create3Hmi.SetName(text) + Create3Hmi.SetNamespace(text) } } diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp index 93821f1e..2d26a219 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp @@ -12,7 +12,7 @@ InterfaceButtons::InterfaceButtons() : rclcpp::Node("sensors_node") { interface_buttons_sub_ = this->create_subscription( - "_create3_buttons", + "_internal/create3_buttons", rclcpp::SensorDataQoS(), std::bind(&InterfaceButtons::create3_buttons_callback, this, std::placeholders::_1)); From 2563047d6affef6eae34c3e1a673e13890af355f Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Mon, 13 Mar 2023 17:58:46 -0400 Subject: [PATCH 07/14] Frame id fixes --- .../irobot_create_control/launch/include/control.py | 5 ++++- .../src/pose_republisher/pose_republisher_node.cpp | 6 ++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/irobot_create_common/irobot_create_control/launch/include/control.py b/irobot_create_common/irobot_create_control/launch/include/control.py index a07763c7..c7af23a4 100644 --- a/irobot_create_common/irobot_create_control/launch/include/control.py +++ b/irobot_create_common/irobot_create_control/launch/include/control.py @@ -6,6 +6,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import LaunchConfigurationNotEquals from launch.event_handlers import OnProcessExit from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -58,12 +59,13 @@ def generate_launch_description(): name='tf_namespaced_odom_publisher', arguments=['0', '0', '0', '0', '0', '0', - [LaunchConfiguration('namespace'), '/odom'], 'odom'], + 'odom', [LaunchConfiguration('namespace'), '/odom']], remappings=[ ('/tf', 'tf'), ('/tf_static', 'tf_static') ], output='screen', + condition=LaunchConfigurationNotEquals('namespace', '') ) # Static transform from /base_link to base_link @@ -79,6 +81,7 @@ def generate_launch_description(): ('/tf_static', 'tf_static') ], output='screen', + condition=LaunchConfigurationNotEquals('namespace', '') ) ld = LaunchDescription(ARGUMENTS) diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp index 7659cb53..30c9dde8 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp @@ -72,10 +72,8 @@ PoseRepublisher::PoseRepublisher() "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); - // Standard dock frame id is namespaced - standard_dock_frame_id_ = get_effective_namespace() + "/standard_dock"; - // Remove leading '/' - standard_dock_frame_id_.erase(0, 1); + // Standard dock frame id + standard_dock_frame_id_ = robot_name_ + "/standard_dock"; } void PoseRepublisher::robot_subscriber_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) From d62634a1f8f52df4b93c55d4a318f4c045a5e98c Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 15 Mar 2023 16:23:08 -0400 Subject: [PATCH 08/14] Changed robot name to be /create3, rather than using a robot_name arg Changed dock name to be /standard_dock Remove robot_name arg, use just namespace for everything --- .../irobot_create_common_bringup/namespace.py | 33 +++++++++++++++++++ .../launch/create3_nodes.launch.py | 6 ++-- .../launch/robot_description.launch.py | 4 +-- .../launch/include/control.py | 8 ++--- .../launch/create3_ignition.launch.py | 3 -- .../launch/create3_ignition_nodes.launch.py | 5 ++- .../create3_ros_ignition_bridge.launch.py | 9 +++-- .../launch/create3_spawn.launch.py | 31 ++++++++++++----- .../worlds/depot.sdf | 2 +- .../pose_republisher/pose_republisher.hpp | 2 +- .../pose_republisher_node.cpp | 7 ++-- 11 files changed, 77 insertions(+), 33 deletions(-) create mode 100644 irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/namespace.py diff --git a/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/namespace.py b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/namespace.py new file mode 100644 index 00000000..4f4a833a --- /dev/null +++ b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/namespace.py @@ -0,0 +1,33 @@ +from typing import Union + +from launch import LaunchContext, SomeSubstitutionsType, Substitution + + +class GetNamespacedName(Substitution): + def __init__( + self, + namespace: Union[SomeSubstitutionsType, str], + name: Union[SomeSubstitutionsType, str] + ) -> None: + self.__namespace = namespace + self.__name = name + + def perform( + self, + context: LaunchContext = None, + ) -> str: + if isinstance(self.__namespace, Substitution): + namespace = str(self.__namespace.perform(context)) + else: + namespace = str(self.__namespace) + + if isinstance(self.__name, Substitution): + name = str(self.__name.perform(context)) + else: + name = str(self.__name) + + if namespace == '': + namespaced_name = name + else: + namespaced_name = namespace + '/' + name + return namespaced_name diff --git a/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py index ce5cd68e..ca2833b5 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py @@ -15,8 +15,8 @@ DeclareLaunchArgument('gazebo', default_value='classic', choices=['classic', 'ignition'], description='Which gazebo simulator to use'), - DeclareLaunchArgument('robot_name', default_value='create3', - description='Robot name'), + DeclareLaunchArgument('namespace', default_value='', + description='Robot namespace'), ] @@ -47,7 +47,7 @@ def generate_launch_description(): # Includes diffdrive_controller = IncludeLaunchDescription( PythonLaunchDescriptionSource([control_launch_file]), - launch_arguments=[('robot_name', LaunchConfiguration('robot_name'))] + launch_arguments=[('namespace', LaunchConfiguration('namespace'))] ) # Publish hazards vector diff --git a/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py index 48b15e49..54b2637e 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/robot_description.launch.py @@ -18,9 +18,7 @@ DeclareLaunchArgument('visualize_rays', default_value='false', choices=['true', 'false'], description='Enable/disable ray visualization'), - DeclareLaunchArgument('robot_name', default_value='create3', - description='Robot name'), - DeclareLaunchArgument('namespace', default_value=LaunchConfiguration('robot_name'), + DeclareLaunchArgument('namespace', default_value='', description='Robot namespace'), ] diff --git a/irobot_create_common/irobot_create_control/launch/include/control.py b/irobot_create_common/irobot_create_control/launch/include/control.py index c7af23a4..89cba524 100644 --- a/irobot_create_common/irobot_create_control/launch/include/control.py +++ b/irobot_create_common/irobot_create_control/launch/include/control.py @@ -12,9 +12,7 @@ from launch_ros.actions import Node ARGUMENTS = [ - DeclareLaunchArgument('robot_name', default_value='create3', - description='Robot name'), - DeclareLaunchArgument('namespace', default_value=LaunchConfiguration('robot_name'), + DeclareLaunchArgument('namespace', default_value='', description='Robot namespace'), ] @@ -59,7 +57,7 @@ def generate_launch_description(): name='tf_namespaced_odom_publisher', arguments=['0', '0', '0', '0', '0', '0', - 'odom', [LaunchConfiguration('namespace'), '/odom']], + 'odom', [namespace, '/odom']], remappings=[ ('/tf', 'tf'), ('/tf_static', 'tf_static') @@ -75,7 +73,7 @@ def generate_launch_description(): name='tf_namespaced_base_link_publisher', arguments=['0', '0', '0', '0', '0', '0', - [LaunchConfiguration('namespace'), '/base_link'], 'base_link'], + [namespace, '/base_link'], 'base_link'], remappings=[ ('/tf', 'tf'), ('/tf_static', 'tf_static') diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py index 9ec75bde..b7baa641 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py @@ -11,8 +11,6 @@ ARGUMENTS = [ - DeclareLaunchArgument('robot_name', default_value='create3', - description='Robot name'), DeclareLaunchArgument('namespace', default_value='', description='Robot namespace'), DeclareLaunchArgument('use_rviz', default_value='true', @@ -48,7 +46,6 @@ def generate_launch_description(): robot_spawn = IncludeLaunchDescription( PythonLaunchDescriptionSource([robot_spawn_launch]), launch_arguments=[ - ('robot_name', LaunchConfiguration('robot_name')), ('namespace', LaunchConfiguration('namespace')), ('use_rviz', LaunchConfiguration('use_rviz')), ('x', LaunchConfiguration('x')), diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py index 21f3488f..a2662d46 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition_nodes.launch.py @@ -10,7 +10,9 @@ ARGUMENTS = [ DeclareLaunchArgument('robot_name', default_value='create3', - description='Robot name') + description='Robot name'), + DeclareLaunchArgument('dock_name', default_value='standard_dock', + description='Dock name') ] @@ -31,6 +33,7 @@ def generate_launch_description(): executable='pose_republisher_node', parameters=[pose_republisher_params_yaml_file, {'robot_name': LaunchConfiguration('robot_name')}, + {'dock_name': LaunchConfiguration('dock_name')}, {'use_sim_time': True}], output='screen', ) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py index a85de569..3e982764 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py @@ -13,7 +13,9 @@ description='Use sim time'), DeclareLaunchArgument('robot_name', default_value='create3', description='Ignition model name'), - DeclareLaunchArgument('namespace', default_value=LaunchConfiguration('robot_name'), + DeclareLaunchArgument('dock_name', default_value='standard_dock', + description='Ignition model name'), + DeclareLaunchArgument('namespace', default_value='', description='Robot namespace'), DeclareLaunchArgument('world', default_value='depot', description='World name') @@ -23,6 +25,7 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') robot_name = LaunchConfiguration('robot_name') + dock_name = LaunchConfiguration('dock_name') namespace = LaunchConfiguration('namespace') world = LaunchConfiguration('world') @@ -76,14 +79,14 @@ def generate_launch_description(): ['/model/', robot_name, '/pose' + '@tf2_msgs/msg/TFMessage' + '[ignition.msgs.Pose_V'], - ['/model/', robot_name, '/standard_dock/pose' + + ['/model/', dock_name, '/pose' + '@tf2_msgs/msg/TFMessage' + '[ignition.msgs.Pose_V'] ], remappings=[ (['/model/', robot_name, '/pose'], '_internal/sim_ground_truth_pose'), - (['/model/', robot_name, '/standard_dock/pose'], + (['/model/', dock_name, '/pose'], '_internal/sim_ground_truth_dock_pose') ]) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py index 427e547c..9e2645fa 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py @@ -4,6 +4,7 @@ from ament_index_python.packages import get_package_share_directory from irobot_create_common_bringup.offset import OffsetParser, RotationalOffsetX, RotationalOffsetY +from irobot_create_common_bringup.namespace import GetNamespacedName from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction @@ -23,9 +24,7 @@ description='use_sim_time'), DeclareLaunchArgument('world', default_value='depot', description='Ignition World'), - DeclareLaunchArgument('robot_name', default_value='create3', - description='Robot name'), - DeclareLaunchArgument('namespace', default_value=LaunchConfiguration('robot_name'), + DeclareLaunchArgument('namespace', default_value='', description='Robot namespace'), DeclareLaunchArgument('use_rviz', default_value='true', choices=['true', 'false'], description='Start rviz.'), @@ -66,6 +65,9 @@ def generate_launch_description(): x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') yaw = LaunchConfiguration('yaw') + robot_name = GetNamespacedName(namespace, 'create3') + dock_name = GetNamespacedName(namespace, 'standard_dock') + # Calculate dock offset due to yaw rotation dock_offset_x = RotationalOffsetX(0.157, yaw) dock_offset_y = RotationalOffsetY(0.157, yaw) @@ -96,20 +98,20 @@ def generate_launch_description(): Node( package='ros_ign_gazebo', executable='create', - arguments=['-name', LaunchConfiguration('robot_name'), + arguments=['-name', robot_name, '-x', x, '-y', y, '-z', z, '-Y', yaw, '-topic', 'robot_description'], - output='screen' + output='screen', ), # Spawn dock Node( package='ros_ign_gazebo', executable='create', - arguments=['-name', [LaunchConfiguration('robot_name'), '/standard_dock'], + arguments=['-name', dock_name, '-x', x_dock, '-y', y_dock, '-z', z, @@ -122,17 +124,28 @@ def generate_launch_description(): # ROS Ign Bridge IncludeLaunchDescription( PythonLaunchDescriptionSource([ros_ign_bridge_launch]), - launch_arguments=[('world', LaunchConfiguration('world'))] + launch_arguments=[ + ('world', LaunchConfiguration('world')), + ('robot_name', robot_name), + ('dock_name', dock_name), + ] ), # Create 3 nodes IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_nodes_launch]) + PythonLaunchDescriptionSource([create3_nodes_launch]), + launch_arguments=[ + ('robot_name', robot_name) + ] ), # Create 3 Ignition nodes IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_ignition_nodes_launch]) + PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), + launch_arguments=[ + ('robot_name', robot_name), + ('dock_name', dock_name), + ] ), # Rviz diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf b/irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf index d6625538..810c1310 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf +++ b/irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf @@ -5,7 +5,7 @@ false - 0.002 + 0.01 1.0 diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp b/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp index d7171fa2..e92594e6 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/pose_republisher/pose_republisher.hpp @@ -46,8 +46,8 @@ class PoseRepublisher : public rclcpp::Node tf2::Transform last_dock_pose_; std::string robot_name_; + std::string dock_name_; std::string wheel_joints_[2]; - std::string standard_dock_frame_id_; }; } // namespace irobot_create_ignition_toolbox diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp index 30c9dde8..632ba162 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/pose_republisher/pose_republisher_node.cpp @@ -17,6 +17,8 @@ PoseRepublisher::PoseRepublisher() { robot_name_ = this->declare_parameter("robot_name", "create3"); + dock_name_ = + this->declare_parameter("dock_name", "standard_dock"); std::string robot_pub_topic = this->declare_parameter("robot_publisher_topic", "sim_ground_truth_pose"); std::string robot_sub_topic = @@ -71,9 +73,6 @@ PoseRepublisher::PoseRepublisher() dynamic_joint_state_publisher_ = create_publisher( "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); - - // Standard dock frame id - standard_dock_frame_id_ = robot_name_ + "/standard_dock"; } void PoseRepublisher::robot_subscriber_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) @@ -119,7 +118,7 @@ void PoseRepublisher::dock_subscriber_callback(const tf2_msgs::msg::TFMessage::S { for (uint16_t i = 0; i < msg->transforms.size(); i++) { // Child frame is model name - if (msg->transforms[i].child_frame_id == standard_dock_frame_id_) { + if (msg->transforms[i].child_frame_id == dock_name_) { auto odom_msg = utils::tf_message_to_odom(msg, i); // Save dock pose tf2::convert(odom_msg->pose.pose, last_dock_pose_); From 0676235d9428d6efff3401e526d4118d922d6ede Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Fri, 17 Mar 2023 17:40:34 -0400 Subject: [PATCH 09/14] Fixed bumper Use ogre2 as sensor render engine Reduce max_step_size to 3ms --- README.md | 6 +++--- .../irobot_create_description/urdf/bumper.urdf.xacro | 10 +++++++++- .../urdf/create3.urdf.xacro | 3 ++- .../urdf/sensors/ir_opcode_receivers.urdf.xacro | 2 +- .../launch/create3_ros_ignition_bridge.launch.py | 12 +++++++----- .../launch/create3_spawn.launch.py | 2 +- .../irobot_create_ignition_bringup/worlds/depot.sdf | 3 ++- .../sensors/bumper.hpp | 6 +++--- .../src/sensors/bumper.cpp | 4 ++-- 9 files changed, 30 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index 205b2fd7..aff4b9f3 100644 --- a/README.md +++ b/README.md @@ -25,13 +25,13 @@ Besides the aforementioned dependencies you will also need at least one among Ig Install [Gazebo 11](http://gazebosim.org/tutorials?tut=install_ubuntu) -#### Ignition Edifice +#### Ignition Fortress ```bash sudo apt-get update && sudo apt-get install wget sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - -sudo apt-get update && sudo apt-get install ignition-edifice +sudo apt-get update && sudo apt-get install ignition-fortress ``` ## Build @@ -55,7 +55,7 @@ rosdep install --from-path src -yi - Build the workspace with: ```bash -export IGNITION_VERSION=edifice +export IGNITION_VERSION=fortress colcon build --symlink-install source install/local_setup.bash ``` diff --git a/irobot_create_common/irobot_create_description/urdf/bumper.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/bumper.urdf.xacro index 3f2bfe64..10c1ff7e 100644 --- a/irobot_create_common/irobot_create_description/urdf/bumper.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/bumper.urdf.xacro @@ -15,7 +15,7 @@ functional, while the 4 remaining ones are created as dummy links. --> - @@ -51,6 +51,14 @@ functional, while the 4 remaining ones are created as dummy links. --> ${update_rate} ${link_name}_collision + + + /bumper_contact + + + ${namespace}/bumper_contact + + diff --git a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro index e82d117a..951a1acf 100644 --- a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro @@ -86,6 +86,7 @@ @@ -293,7 +294,7 @@ - ogre + ogre2 diff --git a/irobot_create_common/irobot_create_description/urdf/sensors/ir_opcode_receivers.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/sensors/ir_opcode_receivers.urdf.xacro index 597c30f7..90f39aa7 100644 --- a/irobot_create_common/irobot_create_description/urdf/sensors/ir_opcode_receivers.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/sensors/ir_opcode_receivers.urdf.xacro @@ -14,7 +14,7 @@ sensor_1_range samples:=50 min_range:=0.015 - update_rate:=31 + update_rate:=62 visualize:=false *origin" > diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py index 3e982764..32d4e1f4 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py @@ -114,13 +114,13 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/model/', robot_name, + [namespace, '/bumper_contact' + - '@ros_ign_interfaces/msg/Contacts' + + '@ros_gz_interfaces/msg/Contacts' + '[ignition.msgs.Contacts'] ], remappings=[ - (['/model/', robot_name, + ([namespace, '/bumper_contact'], 'bumper_contact') ]) @@ -131,7 +131,8 @@ def generate_launch_description(): name=cliff + '_bridge', output='screen', parameters=[{ - 'use_sim_time': use_sim_time + 'use_sim_time': use_sim_time, + 'lazy': True }], arguments=[ ['/world/', world, @@ -153,7 +154,8 @@ def generate_launch_description(): name=ir + '_bridge', output='screen', parameters=[{ - 'use_sim_time': use_sim_time + 'use_sim_time': use_sim_time, + 'lazy': True }], arguments=[ ['/world/', world, diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py index 9e2645fa..221fbba2 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py @@ -135,7 +135,7 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource([create3_nodes_launch]), launch_arguments=[ - ('robot_name', robot_name) + ('namespace', namespace) ] ), diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf b/irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf index 810c1310..1e112108 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf +++ b/irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf @@ -5,12 +5,13 @@ false - 0.01 + 0.003 1.0 + diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/bumper.hpp b/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/bumper.hpp index 500d257a..08165cee 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/bumper.hpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/include/irobot_create_ignition_toolbox/sensors/bumper.hpp @@ -14,7 +14,7 @@ #include "irobot_create_msgs/msg/hazard_detection.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" -#include "ros_ign_interfaces/msg/contacts.hpp" +#include "ros_gz_interfaces/msg/contacts.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace irobot_create_ignition_toolbox @@ -27,12 +27,12 @@ class Bumper virtual ~Bumper() {} private: - void bumper_callback(const ros_ign_interfaces::msg::Contacts::SharedPtr bumper_contact_msg); + void bumper_callback(const ros_gz_interfaces::msg::Contacts::SharedPtr bumper_contact_msg); void robot_pose_callback(nav_msgs::msg::Odometry::ConstSharedPtr msg); std::shared_ptr nh_; - rclcpp::Subscription::SharedPtr bumper_sub_; + rclcpp::Subscription::SharedPtr bumper_sub_; rclcpp::Subscription::SharedPtr robot_pose_sub_; rclcpp::Publisher::SharedPtr hazard_pub_; diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/bumper.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/bumper.cpp index 5c71be12..30b76a05 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/bumper.cpp +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/src/sensors/bumper.cpp @@ -17,7 +17,7 @@ using irobot_create_ignition_toolbox::Bumper; Bumper::Bumper(std::shared_ptr & nh) : nh_(nh) { - bumper_sub_ = nh_->create_subscription( + bumper_sub_ = nh_->create_subscription( "bumper_contact", rclcpp::SensorDataQoS(), std::bind(&Bumper::bumper_callback, this, std::placeholders::_1)); @@ -31,7 +31,7 @@ Bumper::Bumper(std::shared_ptr & nh) "_internal/bumper/event", rclcpp::SensorDataQoS()); } -void Bumper::bumper_callback(const ros_ign_interfaces::msg::Contacts::SharedPtr bumper_contact_msg) +void Bumper::bumper_callback(const ros_gz_interfaces::msg::Contacts::SharedPtr bumper_contact_msg) { tf2::Transform robot_pose(tf2::Transform::getIdentity()); { From 78c4e77a3bf80191e2c0b9016f765d651f084ae5 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Mon, 3 Apr 2023 12:02:31 -0400 Subject: [PATCH 10/14] Linter fixes --- .../urdf/create3.urdf.xacro | 2 +- .../launch/create3_ros_ignition_bridge.launch.py | 12 ++++++------ .../Create3Hmi/Create3Hmi.cc | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro index 951a1acf..92acc617 100644 --- a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro @@ -294,7 +294,7 @@ - ogre2 + ogre diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py index 32d4e1f4..ced37aae 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py @@ -57,15 +57,15 @@ def generate_launch_description(): }], arguments=[ [namespace, - '/cmd_vel' + '@geometry_msgs/msg/Twist' + '[ignition.msgs.Twist'], + '/cmd_vel' + '@geometry_msgs/msg/Twist' + '[ignition.msgs.Twist'], ['/model/', robot_name, '/cmd_vel' + - '@geometry_msgs/msg/Twist' + - ']ignition.msgs.Twist'] + '@geometry_msgs/msg/Twist' + + ']ignition.msgs.Twist'] ], remappings=[ ([namespace, '/cmd_vel'], 'cmd_vel'), (['/model/', robot_name, '/cmd_vel'], - 'diffdrive_controller/cmd_vel_unstamped') + 'diffdrive_controller/cmd_vel_unstamped') ]) # Pose bridge @@ -182,8 +182,8 @@ def generate_launch_description(): }], arguments=[ [namespace, '/create3_buttons' + - '@std_msgs/msg/Int32' + - '[ignition.msgs.Int32'], + '@std_msgs/msg/Int32' + + '[ignition.msgs.Int32'], ], remappings=[ ([namespace, '/create3_buttons'], '_internal/create3_buttons'), diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc index cae270e5..78877aad 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc @@ -71,7 +71,7 @@ void Create3Hmi::SetNamespace(const QString &_name) { this->namespace_ = _name.toStdString(); this->create3_button_topic_ = this->namespace_ + "/create3_buttons"; - + ignmsg << "A new robot name has been entered, publishing on topic: '" << this->create3_button_topic_ << " ' " < Date: Mon, 3 Apr 2023 12:56:02 -0400 Subject: [PATCH 11/14] Fixed import order --- .../launch/create3_spawn.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py index 221fbba2..261bda34 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py @@ -3,8 +3,8 @@ from ament_index_python.packages import get_package_share_directory -from irobot_create_common_bringup.offset import OffsetParser, RotationalOffsetX, RotationalOffsetY from irobot_create_common_bringup.namespace import GetNamespacedName +from irobot_create_common_bringup.offset import OffsetParser, RotationalOffsetX, RotationalOffsetY from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction From c39b120d3546b32fdd82031afe7e03582c7d33db Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 2 May 2023 19:05:19 -0400 Subject: [PATCH 12/14] Multi robot support for gazebo classic --- .../launch/dock_description.launch.py | 7 +- .../urdf/bumper.urdf.xacro | 2 +- .../urdf/create3.urdf.xacro | 42 ++-- .../urdf/dock/standard_dock.urdf.xacro | 3 +- .../urdf/sensors/cliff_sensor.urdf.xacro | 4 +- .../urdf/sensors/imu.urdf.xacro | 4 +- .../urdf/sensors/ir_intensity.urdf.xacro | 4 +- .../sensors/ir_opcode_receivers.urdf.xacro | 3 +- .../urdf/sensors/optical_mouse.urdf.xacro | 4 +- .../urdf/wheel_drop.urdf.xacro | 4 +- .../urdf/wheel_with_wheeldrop.urdf.xacro | 4 +- .../launch/create3_gazebo.launch.py | 180 +++--------------- .../launch/create3_spawn.launch.py | 142 ++++++++++++++ .../launch/gazebo.launch.py | 83 ++++++++ .../src/gazebo_ros_bumper.cpp | 6 +- .../src/gazebo_ros_docking_status.cpp | 2 +- 16 files changed, 306 insertions(+), 188 deletions(-) create mode 100644 irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_spawn.launch.py create mode 100644 irobot_create_gazebo/irobot_create_gazebo_bringup/launch/gazebo.launch.py diff --git a/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py index 0c632055..335e2ac3 100644 --- a/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py +++ b/irobot_create_common/irobot_create_common_bringup/launch/dock_description.launch.py @@ -15,7 +15,9 @@ description='Which gazebo simulation to use'), DeclareLaunchArgument('visualize_rays', default_value='true', choices=['true', 'false'], - description='Enable/disable ray visualization') + description='Enable/disable ray visualization'), + DeclareLaunchArgument('namespace', default_value='', + description='Robot namespace'), ] @@ -28,8 +30,8 @@ def generate_launch_description(): # Launch Configurations visualize_rays = LaunchConfiguration('visualize_rays') - gazebo_simulator = LaunchConfiguration('gazebo') + namespace = LaunchConfiguration('namespace') state_publisher = Node( package='robot_state_publisher', @@ -42,6 +44,7 @@ def generate_launch_description(): Command( ['xacro', ' ', dock_xacro_file, ' ', 'gazebo:=', gazebo_simulator, ' ', + 'namespace:=', namespace, ' ', 'visualize_rays:=', visualize_rays])}, ], remappings=[ diff --git a/irobot_create_common/irobot_create_description/urdf/bumper.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/bumper.urdf.xacro index 10c1ff7e..03d67e69 100644 --- a/irobot_create_common/irobot_create_description/urdf/bumper.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/bumper.urdf.xacro @@ -63,7 +63,7 @@ functional, while the 4 remaining ones are created as dummy links. --> - / + ${namespace} ~/out:=_internal/bumper/event diff --git a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro index 92acc617..cd0003b9 100644 --- a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro @@ -99,11 +99,11 @@ - + - + @@ -113,16 +113,19 @@ - + - $(find irobot_create_control)/config/control.yaml + $(find irobot_create_control)/config/control.yaml ~/odom:=odom + /tf:=tf + /tf_static:=tf_static + /diagnostics:=diagnostics $(arg namespace) @@ -145,7 +148,7 @@ - + @@ -162,22 +165,22 @@ - + - + - + - + @@ -199,25 +202,25 @@ left front_right side_left right --> - + - + - + - + - + - + - + @@ -238,7 +241,8 @@ - @@ -250,7 +254,7 @@ - / + $(arg namespace) odom:=sim_ground_truth_pose base_link @@ -266,7 +270,7 @@ - / + $(arg namespace) ~/out:=dock_status 1.0 diff --git a/irobot_create_common/irobot_create_description/urdf/dock/standard_dock.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/dock/standard_dock.urdf.xacro index 76cbbf3b..7e19ea63 100644 --- a/irobot_create_common/irobot_create_description/urdf/dock/standard_dock.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/dock/standard_dock.urdf.xacro @@ -5,6 +5,7 @@ + @@ -88,7 +89,7 @@ - / + $(arg namespace) odom:=sim_ground_truth_dock_pose ${link_name} diff --git a/irobot_create_common/irobot_create_description/urdf/sensors/cliff_sensor.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/sensors/cliff_sensor.urdf.xacro index 27f10256..26542dcd 100644 --- a/irobot_create_common/irobot_create_description/urdf/sensors/cliff_sensor.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/sensors/cliff_sensor.urdf.xacro @@ -1,7 +1,7 @@ - @@ -31,7 +31,7 @@ r_min="${min_range}" r_max="${max_range}" r_res="1.0"> - / + ${namespace} ~/out:=_internal/${sensor_name}/event ${detection_threshold} diff --git a/irobot_create_common/irobot_create_description/urdf/sensors/imu.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/sensors/imu.urdf.xacro index aecf92f3..738bf13a 100644 --- a/irobot_create_common/irobot_create_description/urdf/sensors/imu.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/sensors/imu.urdf.xacro @@ -1,7 +1,7 @@ - + @@ -62,7 +62,7 @@ - / + ${namespace} ~/out:=imu diff --git a/irobot_create_common/irobot_create_description/urdf/sensors/ir_intensity.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/sensors/ir_intensity.urdf.xacro index de9d51f5..6c21bd36 100644 --- a/irobot_create_common/irobot_create_description/urdf/sensors/ir_intensity.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/sensors/ir_intensity.urdf.xacro @@ -3,7 +3,7 @@ @@ -32,7 +32,7 @@ r_min="${min_range}" r_max="${max_range}" r_res="1.0"> - / + ${namespace} ~/out:=_internal/${link_name} diff --git a/irobot_create_common/irobot_create_description/urdf/sensors/ir_opcode_receivers.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/sensors/ir_opcode_receivers.urdf.xacro index 90f39aa7..4aa46011 100644 --- a/irobot_create_common/irobot_create_description/urdf/sensors/ir_opcode_receivers.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/sensors/ir_opcode_receivers.urdf.xacro @@ -4,6 +4,7 @@ - / + ${namespace} ~/out:=ir_opcode ${update_rate} diff --git a/irobot_create_common/irobot_create_description/urdf/sensors/optical_mouse.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/sensors/optical_mouse.urdf.xacro index f22583be..5b917f8d 100644 --- a/irobot_create_common/irobot_create_description/urdf/sensors/optical_mouse.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/sensors/optical_mouse.urdf.xacro @@ -1,7 +1,7 @@ - + @@ -17,7 +17,7 @@ - / + ${namespace} ~/out:=${name} ${update_rate} diff --git a/irobot_create_common/irobot_create_description/urdf/wheel_drop.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/wheel_drop.urdf.xacro index 796abe21..1fa6fd31 100644 --- a/irobot_create_common/irobot_create_description/urdf/wheel_drop.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/wheel_drop.urdf.xacro @@ -1,6 +1,6 @@ - + @@ -52,7 +52,7 @@ - / + $(arg namespace) ~/out:=_internal/wheel_drop/${name}_wheel/event ${update_rate} diff --git a/irobot_create_common/irobot_create_description/urdf/wheel_with_wheeldrop.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/wheel_with_wheeldrop.urdf.xacro index d96e9a86..2c88579a 100644 --- a/irobot_create_common/irobot_create_description/urdf/wheel_with_wheeldrop.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/wheel_with_wheeldrop.urdf.xacro @@ -1,12 +1,12 @@ - + - + diff --git a/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_gazebo.launch.py b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_gazebo.launch.py index fd9a5324..3b73512c 100644 --- a/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_gazebo.launch.py +++ b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_gazebo.launch.py @@ -4,35 +4,12 @@ # # Launch Create(R) 3 in Gazebo and optionally also in RViz. -import os - -from pathlib import Path - from ament_index_python.packages import get_package_share_directory -from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution -from launch.actions import DeclareLaunchArgument, ExecuteProcess -from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable -from launch.conditions import IfCondition +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node - - -class OffsetParser(Substitution): - def __init__( - self, - number: SomeSubstitutionsType, - offset: float, - ) -> None: - self.__number = number - self.__offset = offset - - def perform( - self, - context: LaunchContext = None, - ) -> str: - number = float(self.__number.perform(context)) - return f'{number + self.__offset}' +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution ARGUMENTS = [ @@ -47,6 +24,8 @@ def perform( description='Spawn the standard dock model.'), DeclareLaunchArgument('world_path', default_value='', description='Set world path, by default is empty.world'), + DeclareLaunchArgument('namespace', default_value='', + description='Robot namespace'), ] for pose_element in ['x', 'y', 'z', 'yaw']: @@ -54,137 +33,38 @@ def perform( description=f'{pose_element} component of the robot pose.')) -# Rviz requires US locale to correctly display the wheels -os.environ['LC_NUMERIC'] = 'en_US.UTF-8' - - def generate_launch_description(): # Directories - pkg_create3_common_bringup = get_package_share_directory('irobot_create_common_bringup') pkg_create3_gazebo_bringup = get_package_share_directory('irobot_create_gazebo_bringup') - pkg_irobot_create_description = get_package_share_directory('irobot_create_description') - - # Set ignition resource path - gz_resource_path = SetEnvironmentVariable(name='GAZEBO_MODEL_PATH', value=[ - EnvironmentVariable('GAZEBO_MODEL_PATH', - default_value=''), - '/usr/share/gazebo-11/models/:', - str(Path(pkg_irobot_create_description). - parent.resolve())]) - - # Set GAZEBO_MODEL_URI to empty string to prevent Gazebo from downloading models - gz_model_uri = SetEnvironmentVariable(name='GAZEBO_MODEL_URI', value=['']) # Paths - create3_nodes_launch_file = PathJoinSubstitution( - [pkg_create3_common_bringup, 'launch', 'create3_nodes.launch.py']) - dock_description_launch_file = PathJoinSubstitution( - [pkg_create3_common_bringup, 'launch', 'dock_description.launch.py']) - robot_description_launch_file = PathJoinSubstitution( - [pkg_create3_common_bringup, 'launch', 'robot_description.launch.py']) - rviz2_launch_file = PathJoinSubstitution( - [pkg_create3_common_bringup, 'launch', 'rviz2.launch.py']) - - gazebo_params_yaml_file = os.path.join( - pkg_create3_gazebo_bringup, 'config', 'gazebo_params.yaml') - - # Launch configurations - x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') - yaw = LaunchConfiguration('yaw') - world_path = LaunchConfiguration('world_path') - spawn_dock = LaunchConfiguration('spawn_dock') - use_gazebo_gui = LaunchConfiguration('use_gazebo_gui') - use_rviz = LaunchConfiguration('use_rviz') - - # Gazebo server - gzserver = ExecuteProcess( - cmd=['gzserver', - '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', - world_path, - 'extra-gazebo-args', '--ros-args', '--params-file', gazebo_params_yaml_file], - output='screen', + gazebo_launch = PathJoinSubstitution( + [pkg_create3_gazebo_bringup, 'launch', 'gazebo.launch.py']) + robot_spawn_launch = PathJoinSubstitution( + [pkg_create3_gazebo_bringup, 'launch', 'create3_spawn.launch.py']) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([gazebo_launch]), + launch_arguments=[ + ('world_path', LaunchConfiguration('world_path')), + ('use_gazebo_gui', LaunchConfiguration('use_gazebo_gui')) + ] ) - # Gazebo client - gzclient = ExecuteProcess( - cmd=['gzclient'], - output='screen', - condition=IfCondition(use_gazebo_gui), - ) - - # Dock model and description - # The robot starts docked. So we spawn the dock at the robot position. - # We need to include an offset on the x and yaw to correctly "plug" the robot. - x_dock = OffsetParser(x, 0.157) - yaw_dock = OffsetParser(yaw, 3.1416) - dock_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource([dock_description_launch_file]), - condition=IfCondition(spawn_dock), - # The robot starts docked - launch_arguments={'x': x_dock, 'y': y, 'z': z, 'yaw': yaw_dock}.items(), - ) - spawn_dock = Node( - package='gazebo_ros', - executable='spawn_entity.py', - name='spawn_standard_dock', - arguments=['-entity', - 'standard_dock', - '-topic', - 'standard_dock_description', - '-x', x_dock, - '-y', y, - '-z', z, - '-Y', yaw_dock], - output='screen', - condition=IfCondition(spawn_dock), - ) - - # Create 3 robot model and description - robot_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource([robot_description_launch_file]) - ) - spawn_robot = Node( - package='gazebo_ros', - executable='spawn_entity.py', - name='spawn_create3', - arguments=['-entity', - 'create3', - '-topic', - 'robot_description', - '-x', x, - '-y', y, - '-z', z, - '-Y', yaw], - output='screen', - ) - - # Create 3 nodes - create3_nodes = IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_nodes_launch_file]) - ) - - # RVIZ2 - rviz2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource([rviz2_launch_file]), - condition=IfCondition(use_rviz), - ) + robot_spawn = IncludeLaunchDescription( + PythonLaunchDescriptionSource([robot_spawn_launch]), + launch_arguments=[ + ('namespace', LaunchConfiguration('namespace')), + ('use_rviz', LaunchConfiguration('use_rviz')), + ('x', LaunchConfiguration('x')), + ('y', LaunchConfiguration('y')), + ('z', LaunchConfiguration('z')), + ('yaw', LaunchConfiguration('yaw'))]) # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) - # Gazebo processes - ld.add_action(gz_resource_path) - ld.add_action(gz_model_uri) - ld.add_action(gzserver) - ld.add_action(gzclient) - # Include robot description - ld.add_action(robot_description) - ld.add_action(spawn_robot) - ld.add_action(spawn_dock) - ld.add_action(dock_description) - # Include Create 3 nodes - ld.add_action(create3_nodes) - # Rviz - ld.add_action(rviz2) - + # Gazebo + ld.add_action(gazebo) + # Robot spawn + ld.add_action(robot_spawn) return ld diff --git a/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_spawn.launch.py b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_spawn.launch.py new file mode 100644 index 00000000..5c0274a2 --- /dev/null +++ b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_spawn.launch.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python3 +# Copyright 2021 iRobot Corporation. All Rights Reserved. +# @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com) +# +# Launch Create(R) 3 in Gazebo and optionally also in RViz. + +import os + +from irobot_create_common_bringup.namespace import GetNamespacedName +from irobot_create_common_bringup.offset import OffsetParser, RotationalOffsetX, RotationalOffsetY + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.actions import IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node, PushRosNamespace + + +ARGUMENTS = [ + DeclareLaunchArgument('use_rviz', default_value='true', + choices=['true', 'false'], + description='Start rviz.'), + DeclareLaunchArgument('spawn_dock', default_value='true', + choices=['true', 'false'], + description='Spawn the standard dock model.'), + DeclareLaunchArgument('namespace', default_value='', + description='Robot namespace'), +] + +for pose_element in ['x', 'y', 'z', 'yaw']: + ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', + description=f'{pose_element} component of the robot pose.')) + + +# Rviz requires US locale to correctly display the wheels +os.environ['LC_NUMERIC'] = 'en_US.UTF-8' + + +def generate_launch_description(): + # Directories + pkg_create3_common_bringup = get_package_share_directory('irobot_create_common_bringup') + + # Paths + create3_nodes_launch_file = PathJoinSubstitution( + [pkg_create3_common_bringup, 'launch', 'create3_nodes.launch.py']) + dock_description_launch_file = PathJoinSubstitution( + [pkg_create3_common_bringup, 'launch', 'dock_description.launch.py']) + robot_description_launch_file = PathJoinSubstitution( + [pkg_create3_common_bringup, 'launch', 'robot_description.launch.py']) + rviz2_launch_file = PathJoinSubstitution( + [pkg_create3_common_bringup, 'launch', 'rviz2.launch.py']) + + # Launch configurations + namespace = LaunchConfiguration('namespace') + x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') + yaw = LaunchConfiguration('yaw') + spawn_dock = LaunchConfiguration('spawn_dock') + use_rviz = LaunchConfiguration('use_rviz') + + robot_name = GetNamespacedName(namespace, 'create3') + dock_name = GetNamespacedName(namespace, 'standard_dock') + + # Calculate dock offset due to yaw rotation + dock_offset_x = RotationalOffsetX(0.157, yaw) + dock_offset_y = RotationalOffsetY(0.157, yaw) + # Spawn dock at robot position + rotational offset + x_dock = OffsetParser(x, dock_offset_x) + y_dock = OffsetParser(y, dock_offset_y) + # Rotate dock towards robot + yaw_dock = OffsetParser(yaw, 3.1416) + + spawn_robot_group_action = GroupAction([ + PushRosNamespace(namespace), + + # Dock description + IncludeLaunchDescription( + PythonLaunchDescriptionSource([dock_description_launch_file]), + launch_arguments={'gazebo': 'classic'}.items(), + condition=IfCondition(spawn_dock), + ), + + # Dock spawn + Node( + package='gazebo_ros', + executable='spawn_entity.py', + name='spawn_standard_dock', + arguments=['-entity', + dock_name, + '-topic', + 'standard_dock_description', + '-x', x_dock, + '-y', y_dock, + '-z', z, + '-Y', yaw_dock], + output='screen', + condition=IfCondition(spawn_dock), + ), + + # Create 3 robot model and description + IncludeLaunchDescription( + PythonLaunchDescriptionSource([robot_description_launch_file]), + launch_arguments={'gazebo': 'classic'}.items(), + ), + + # Create 3 spawn + Node( + package='gazebo_ros', + executable='spawn_entity.py', + name='spawn_create3', + arguments=['-entity', + robot_name, + '-topic', + 'robot_description', + '-x', x, + '-y', y, + '-z', z, + '-Y', yaw], + output='screen', + ), + + # Create 3 nodes + IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_nodes_launch_file]), + launch_arguments=[ + ('namespace', namespace) + ] + ), + + # RVIZ2 + IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz2_launch_file]), + condition=IfCondition(use_rviz), + ), + ]) + + # Define LaunchDescription variable + ld = LaunchDescription(ARGUMENTS) + ld.add_action(spawn_robot_group_action) + return ld diff --git a/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/gazebo.launch.py b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/gazebo.launch.py new file mode 100644 index 00000000..f846722b --- /dev/null +++ b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/gazebo.launch.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +# Copyright 2021 iRobot Corporation. All Rights Reserved. +# @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com) +# +# Launch Create(R) 3 in Gazebo and optionally also in RViz. + +import os + +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.actions import SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import EnvironmentVariable, LaunchConfiguration + + +ARGUMENTS = [ + DeclareLaunchArgument('use_rviz', default_value='true', + choices=['true', 'false'], + description='Start rviz.'), + DeclareLaunchArgument('use_gazebo_gui', default_value='true', + choices=['true', 'false'], + description='Set "false" to run gazebo headless.'), + DeclareLaunchArgument('world_path', default_value='', + description='Set world path, by default is empty.world'), +] + + +# Rviz requires US locale to correctly display the wheels +os.environ['LC_NUMERIC'] = 'en_US.UTF-8' + + +def generate_launch_description(): + # Directories + pkg_create3_gazebo_bringup = get_package_share_directory('irobot_create_gazebo_bringup') + pkg_irobot_create_description = get_package_share_directory('irobot_create_description') + + # Set ignition resource path + gz_resource_path = SetEnvironmentVariable(name='GAZEBO_MODEL_PATH', value=[ + EnvironmentVariable('GAZEBO_MODEL_PATH', + default_value=''), + '/usr/share/gazebo-11/models/:', + str(Path(pkg_irobot_create_description). + parent.resolve())]) + + # Set GAZEBO_MODEL_URI to empty string to prevent Gazebo from downloading models + gz_model_uri = SetEnvironmentVariable(name='GAZEBO_MODEL_URI', value=['']) + + gazebo_params_yaml_file = os.path.join( + pkg_create3_gazebo_bringup, 'config', 'gazebo_params.yaml') + + # Launch configurations + world_path = LaunchConfiguration('world_path') + use_gazebo_gui = LaunchConfiguration('use_gazebo_gui') + + # Gazebo server + gzserver = ExecuteProcess( + cmd=['gzserver', + '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', + world_path, + 'extra-gazebo-args', '--ros-args', '--params-file', gazebo_params_yaml_file], + output='screen', + ) + + # Gazebo client + gzclient = ExecuteProcess( + cmd=['gzclient'], + output='screen', + condition=IfCondition(use_gazebo_gui), + ) + + # Define LaunchDescription variable + ld = LaunchDescription(ARGUMENTS) + # Gazebo processes + ld.add_action(gz_resource_path) + ld.add_action(gz_model_uri) + ld.add_action(gzserver) + ld.add_action(gzclient) + + return ld diff --git a/irobot_create_gazebo/irobot_create_gazebo_plugins/src/gazebo_ros_bumper.cpp b/irobot_create_gazebo/irobot_create_gazebo_plugins/src/gazebo_ros_bumper.cpp index 0e0afe44..28cfe471 100644 --- a/irobot_create_gazebo/irobot_create_gazebo_plugins/src/gazebo_ros_bumper.cpp +++ b/irobot_create_gazebo/irobot_create_gazebo_plugins/src/gazebo_ros_bumper.cpp @@ -81,7 +81,11 @@ void GazeboRosBumper::GzPoseCallback(ConstPosesStampedPtr & msg) // Find in the message's vector a pose element corresponding to the mobile base's absolute pose // identified under the "create3" name. const auto i = std::find_if( - poses.begin(), poses.end(), [](const auto & pose) -> bool {return pose.name() == "create3";}); + poses.begin(), poses.end(), [](const auto & pose) -> bool { + std::string frame("create3"); + // Compare end of name to "create3". This works with namespaced frames. + return std::equal(frame.rbegin(), frame.rend(), pose.name().rbegin()); + }); // If not matches are found, return immediately. if (i == poses.end()) { return; diff --git a/irobot_create_gazebo/irobot_create_gazebo_plugins/src/gazebo_ros_docking_status.cpp b/irobot_create_gazebo/irobot_create_gazebo_plugins/src/gazebo_ros_docking_status.cpp index 6ad39648..21861af5 100644 --- a/irobot_create_gazebo/irobot_create_gazebo_plugins/src/gazebo_ros_docking_status.cpp +++ b/irobot_create_gazebo/irobot_create_gazebo_plugins/src/gazebo_ros_docking_status.cpp @@ -46,7 +46,7 @@ void GazeboRosDockingStatus::Load(gazebo::physics::ModelPtr model, sdf::ElementP "~/out", rclcpp::SensorDataQoS().reliable()); sub_ = ros_node_->create_subscription( - "/ir_opcode", rclcpp::SensorDataQoS(), + "ir_opcode", rclcpp::SensorDataQoS(), std::bind(&GazeboRosDockingStatus::IrOpcodeCb, this, std::placeholders::_1)); // Set message frame_id From 0bfc11d4fdc4a300c86d5c110537449cb8df6751 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 2 May 2023 19:14:55 -0400 Subject: [PATCH 13/14] Updated README --- README.md | 50 +++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 47 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index aff4b9f3..ce8bcefe 100644 --- a/README.md +++ b/README.md @@ -5,8 +5,6 @@ This is a ROS 2 simulation stack for the [iRobot® Create® 3](https://edu.irobot.com/create3) robot. Both Ignition Gazebo and Classic Gazebo are supported. -> :warning: **To run with Ignition Gazebo you must first build and install the [`gz_ros2_control`](https://github.com/ros-controls/gz_ros2_control) package master branch from sources!** - Have a look at the [Create® 3 documentation](https://iroboteducation.github.io/create3_docs/) for more details on the ROS 2 interfaces exposed by the robot. ## Prerequisites @@ -66,12 +64,35 @@ source install/local_setup.bash ##### Empty world -Create® 3 can be spawned in an empty world in Gazebo and monitored through RViz with +Create® 3 can be spawned in an empty world in Gazebo and monitored through RViz with: ```bash ros2 launch irobot_create_gazebo_bringup create3_gazebo.launch.py ``` +The spawn point can be changed with the `x`, `y`, `z` and `yaw` launch arguments: + +```bash +ros2 launch irobot_create_gazebo_bringup create3_gazebo.launch.py x:=1.0 y:=0.5 yaw:=1.5707 +``` + +##### Namespacing + +A namespace can be applied to the robot using the `namespace` launch argument: + +```bash +ros2 launch irobot_create_gazebo_bringup create3_gazebo.launch.py namespace:=my_robot +``` + +Multiple robots can be spawned with unique namespaces: + +```bash +ros2 launch irobot_create_gazebo_bringup create3_gazebo.launch.py namespace:=robot1 +ros2 launch irobot_create_gazebo_bringup create3_spawn.launch.py namespace:=robot2 x:=1.0 +``` + +> :warning: `create3_gazebo.launch.py` should only be used once as it launches the Gazebo simulator itself. Additional robots should be spawned with `create3_spawn.launch.py`. Namespaces and spawn points should be unique for each robot. + ##### AWS house Create® 3 can be spawned in the AWS small house in Gazebo and monitored through RViz. @@ -99,6 +120,29 @@ Create® 3 can be spawned in a demo world in Ignition and monitored through RViz ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py ``` +The spawn point can be changed with the `x`, `y`, `z` and `yaw` launch arguments: + +```bash +ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py x:=1.0 y:=0.5 yaw:=1.5707 +``` + +##### Namespacing + +A namespace can be applied to the robot using the `namespace` launch argument: + +```bash +ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py namespace:=my_robot +``` + +Multiple robots can be spawned with unique namespaces: + +```bash +ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py namespace:=robot1 +ros2 launch irobot_create_ignition_bringup create3_spawn.launch.py namespace:=robot2 x:=1.0 +``` + +> :warning: `create3_ignition.launch.py` should only be used once as it launches the Ignition simulator itself. Additional robots should be spawned with `create3_spawn.launch.py`. Namespaces and spawn points should be unique for each robot. + ## Package layout This repository contains packages for both the Classic and Ignition Gazebo simulators: From 1a0a68404324c35062b4277e19ca0ecedb4443d6 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 2 May 2023 20:45:46 -0400 Subject: [PATCH 14/14] Fixed import order --- .../launch/create3_spawn.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_spawn.launch.py b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_spawn.launch.py index 5c0274a2..9dcb63b4 100644 --- a/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_spawn.launch.py +++ b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_spawn.launch.py @@ -6,10 +6,11 @@ import os +from ament_index_python.packages import get_package_share_directory + from irobot_create_common_bringup.namespace import GetNamespacedName from irobot_create_common_bringup.offset import OffsetParser, RotationalOffsetX, RotationalOffsetY -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction from launch.actions import IncludeLaunchDescription