diff --git a/README.md b/README.md index 205b2fd7..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 @@ -25,13 +23,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 +53,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 ``` @@ -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: 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/config/hazard_vector_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml index 6e4d5691..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 63f25408..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 37906ab4..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 3a300bb7..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 b89d0226..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 34a07346..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 92e5e3a7..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/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/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/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/create3_nodes.launch.py b/irobot_create_common/irobot_create_common_bringup/launch/create3_nodes.launch.py index 50c3cee8..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 @@ -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('namespace', default_value='', + description='Robot namespace'), ] @@ -44,7 +46,8 @@ def generate_launch_description(): # Includes diffdrive_controller = IncludeLaunchDescription( - PythonLaunchDescriptionSource([control_launch_file]) + PythonLaunchDescriptionSource([control_launch_file]), + launch_arguments=[('namespace', LaunchConfiguration('namespace'))] ) # Publish hazards vector @@ -74,6 +77,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..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 @@ -9,19 +9,16 @@ 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') + description='Which gazebo simulation to use'), + DeclareLaunchArgument('visualize_rays', default_value='true', + choices=['true', 'false'], + description='Enable/disable ray visualization'), + DeclareLaunchArgument('namespace', default_value='', + description='Robot namespace'), ] -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', - choices=['true', 'false'], - description='Enable/disable ray visualization')) def generate_launch_description(): @@ -32,11 +29,9 @@ 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') + namespace = LaunchConfiguration('namespace') state_publisher = Node( package='robot_state_publisher', @@ -49,10 +44,13 @@ def generate_launch_description(): Command( ['xacro', ' ', dock_xacro_file, ' ', 'gazebo:=', gazebo_simulator, ' ', + 'namespace:=', namespace, ' ', 'visualize_rays:=', visualize_rays])}, ], remappings=[ ('robot_description', 'standard_dock_description'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static') ], ) @@ -60,11 +58,15 @@ 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'), + ('/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..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 @@ -10,13 +10,16 @@ from launch.substitutions.launch_configuration import LaunchConfiguration from launch_ros.actions import Node + 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('namespace', default_value='', + description='Robot namespace'), ] @@ -25,6 +28,7 @@ def generate_launch_description(): xacro_file = PathJoinSubstitution([pkg_create3_description, 'urdf', 'create3.urdf.xacro']) gazebo_simulator = LaunchConfiguration('gazebo') visualize_rays = LaunchConfiguration('visualize_rays') + namespace = LaunchConfiguration('namespace') robot_state_publisher = Node( package='robot_state_publisher', @@ -37,8 +41,13 @@ def generate_launch_description(): Command( ['xacro', ' ', xacro_file, ' ', 'gazebo:=', gazebo_simulator, ' ', - 'visualize_rays:=', visualize_rays])}, + 'visualize_rays:=', visualize_rays, ' ', + 'namespace:=', namespace])}, ], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ] ) joint_state_publisher = Node( @@ -47,6 +56,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_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/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..89cba524 100644 --- a/irobot_create_common/irobot_create_control/launch/include/control.py +++ b/irobot_create_common/irobot_create_control/launch/include/control.py @@ -5,30 +5,39 @@ 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.conditions import LaunchConfigurationNotEquals from launch.event_handlers import OnProcessExit -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +ARGUMENTS = [ + DeclareLaunchArgument('namespace', default_value='', + description='Robot namespace'), +] + def generate_launch_description(): pkg_create3_control = get_package_share_directory('irobot_create_control') + namespace = LaunchConfiguration('namespace') + control_params_file = PathJoinSubstitution( [pkg_create3_control, 'config', 'control.yaml']) diffdrive_controller_node = Node( package='controller_manager', executable='spawner', + namespace=namespace, # 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', ) @@ -40,9 +49,44 @@ def generate_launch_description(): ) ) - ld = LaunchDescription() + # 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', + 'odom', [namespace, '/odom']], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ], + output='screen', + condition=LaunchConfigurationNotEquals('namespace', '') + ) + + # 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', + [namespace, '/base_link'], 'base_link'], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ], + output='screen', + condition=LaunchConfigurationNotEquals('namespace', '') + ) + + 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_common/irobot_create_description/urdf/bumper.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/bumper.urdf.xacro index 3f2bfe64..03d67e69 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,11 +51,19 @@ functional, while the 4 remaining ones are created as dummy links. --> ${update_rate} ${link_name}_collision + + + /bumper_contact + + + ${namespace}/bumper_contact + + - / + ${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 d351f11b..cd0003b9 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,9 @@ + + + @@ -83,6 +86,7 @@ @@ -95,11 +99,11 @@ - + - + @@ -109,16 +113,20 @@ - + - $(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) @@ -127,16 +135,20 @@ - $(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) - + @@ -153,22 +165,22 @@ - + - + - + - + @@ -190,25 +202,25 @@ left front_right side_left right --> - + - + - + - + - + - + - + @@ -229,7 +241,8 @@ - @@ -241,7 +254,7 @@ - / + $(arg namespace) odom:=sim_ground_truth_pose base_link @@ -257,7 +270,7 @@ - / + $(arg namespace) ~/out:=dock_status 1.0 @@ -289,5 +302,4 @@ - 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 597c30f7..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 @@ @@ -59,7 +60,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_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_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..9dcb63b4 --- /dev/null +++ b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_spawn.launch.py @@ -0,0 +1,143 @@ +#!/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 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 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 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..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..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,6 +103,7 @@ + /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 057c34b3..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 @@ -1,54 +1,22 @@ # 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.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('namespace', default_value='', + description='Robot namespace'), 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('world', default_value='depot', + description='Ignition World'), ] for pose_element in ['x', 'y', 'z', 'yaw']: @@ -59,132 +27,34 @@ 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')]) # 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_launch = PathJoinSubstitution( + [pkg_irobot_create_ignition_bringup, 'launch', 'ignition.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 4', - ' --gui-config ', - PathJoinSubstitution([pkg_irobot_create_ignition_bringup, - 'gui', 'create3', 'gui.config'])]) + ('world', LaunchConfiguration('world')) ] ) - rviz2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource([rviz2_launch]), - 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'))] - ) + 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'))]) # 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(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) + ld.add_action(ignition) + 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 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 c3997cb5..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 @@ -2,8 +2,7 @@ # @author Roni Kreinin (rkreinin@clearpathrobotics.com) from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument, GroupAction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -14,14 +13,21 @@ description='Use sim time'), DeclareLaunchArgument('robot_name', default_value='create3', description='Ignition model 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') ] def generate_launch_description(): - namespace = LaunchConfiguration('robot_name') use_sim_time = LaunchConfiguration('use_sim_time') + robot_name = LaunchConfiguration('robot_name') + dock_name = LaunchConfiguration('dock_name') + namespace = LaunchConfiguration('namespace') + world = LaunchConfiguration('world') cliff_sensors = [ 'cliff_front_left', @@ -40,160 +46,157 @@ 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', - name='cmd_vel_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[ - '/cmd_vel' + '@geometry_msgs/msg/Twist' + '[ignition.msgs.Twist', - ['/model/', LaunchConfiguration('robot_name'), '/cmd_vel' + - '@geometry_msgs/msg/Twist' + - ']ignition.msgs.Twist'] - ], - remappings=[ - (['/model/', LaunchConfiguration('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_ign_bridge', executable='parameter_bridge', - namespace=namespace, + pose_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', name='pose_bridge', output='screen', parameters=[{ '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' + - '@tf2_msgs/msg/TFMessage' + - '[ignition.msgs.Pose_V' + ['/model/', dock_name, '/pose' + + '@tf2_msgs/msg/TFMessage' + + '[ignition.msgs.Pose_V'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/pose'], - '/_internal/sim_ground_truth_pose'), - ('/model/standard_dock/pose', - '/_internal/sim_ground_truth_dock_pose') + (['/model/', robot_name, '/pose'], + '_internal/sim_ground_truth_pose'), + (['/model/', dock_name, '/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=[{ '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 - 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=[{ 'use_sim_time': use_sim_time }], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), + [namespace, '/bumper_contact' + - '@ros_ign_interfaces/msg/Contacts' + + '@ros_gz_interfaces/msg/Contacts' + '[ignition.msgs.Contacts'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), + ([namespace, '/bumper_contact'], - '/bumper_contact') + 'bumper_contact') ]) # Cliff bridge - cliff_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - namespace=namespace, - 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 - ]) - # IR intensity bridge - ir_intensity_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - namespace=namespace, - 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 - ]) + cliff_bridges = GroupAction([ + Node(package='ros_gz_bridge', executable='parameter_bridge', + name=cliff + '_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'lazy': True + }], + 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 bridges + ir_bridges = GroupAction([ + Node(package='ros_gz_bridge', executable='parameter_bridge', + name=ir + '_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'lazy': True + }], + 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_ign_bridge', executable='parameter_bridge', - namespace=namespace, - name='buttons_msg_bridge', - output='screen', - parameters=[{ - 'use_sim_time': use_sim_time - }], - arguments=[ - ['/create3/buttons' + - '@std_msgs/msg/Int32' + - '[ignition.msgs.Int32'] - ]) + 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) - ld.add_action(clock_bridge) ld.add_action(cmd_vel_bridge) 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.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py new file mode 100644 index 00000000..261bda34 --- /dev/null +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py @@ -0,0 +1,161 @@ +# Copyright 2021 Clearpath Robotics, Inc. +# @author Roni Kreinin (rkreinin@clearpathrobotics.com) + +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 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('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('namespace', default_value='', + description='Robot namespace'), + 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']) + rviz2_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'rviz2.launch.py']) + + # Launch configurations + namespace = LaunchConfiguration('namespace') + 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) + # 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]), + condition=IfCondition(LaunchConfiguration('spawn_dock')), + # The robot starts docked + launch_arguments={'gazebo': 'ignition'}.items(), + ), + + # Robot description + IncludeLaunchDescription( + PythonLaunchDescriptionSource([robot_description_launch]), + launch_arguments={'gazebo': 'ignition'}.items() + ), + + # Spawn Create 3 + Node( + package='ros_ign_gazebo', + executable='create', + arguments=['-name', robot_name, + '-x', x, + '-y', y, + '-z', z, + '-Y', yaw, + '-topic', 'robot_description'], + output='screen', + ), + + # Spawn dock + Node( + package='ros_ign_gazebo', + executable='create', + arguments=['-name', dock_name, + '-x', x_dock, + '-y', y_dock, + '-z', z, + '-Y', yaw_dock, + '-topic', 'standard_dock_description'], + output='screen', + condition=IfCondition(LaunchConfiguration('spawn_dock')) + ), + + # ROS Ign Bridge + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ros_ign_bridge_launch]), + launch_arguments=[ + ('world', LaunchConfiguration('world')), + ('robot_name', robot_name), + ('dock_name', dock_name), + ] + ), + + # Create 3 nodes + IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_nodes_launch]), + launch_arguments=[ + ('namespace', namespace) + ] + ), + + # Create 3 Ignition nodes + IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), + launch_arguments=[ + ('robot_name', robot_name), + ('dock_name', dock_name), + ] + ), + + # Rviz + IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz2_launch]), + condition=IfCondition(LaunchConfiguration('use_rviz')), + ) + ]) + + # 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_bringup/launch/ignition.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py new file mode 100644 index 00000000..835cc937 --- /dev/null +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/ignition.launch.py @@ -0,0 +1,84 @@ +# 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 LaunchDescription +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 + + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('world', default_value='depot', + description='Ignition World'), +] + + +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_bringup/worlds/depot.sdf b/irobot_create_ignition/irobot_create_ignition_bringup/worlds/depot.sdf index d6625538..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.002 + 0.003 1.0 + 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..78877aad 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 "ignition/gui/Application.hh" -#include "ignition/gui/MainWindow.hh" -#include "ignition/msgs/int32.pb.h" -#include "ignition/plugin/Register.hh" +#include +#include +#include + namespace ignition { @@ -32,14 +34,17 @@ Create3Hmi::~Create3Hmi() void Create3Hmi::LoadConfig(const tinyxml2::XMLElement * _pluginElem) { - if (!_pluginElem) { - return; - } - if (this->title.empty()) { this->title = "Create3 HMI"; } + if (_pluginElem) + { + auto namespaceElem = _pluginElem->FirstChildElement("namespace"); + if (nullptr != namespaceElem && nullptr != namespaceElem->GetText()) + this->SetNamespace(namespaceElem->GetText()); + } + this->connect( this, SIGNAL(AddMsg(QString)), this, SLOT(OnAddMsg(QString)), Qt::QueuedConnection); @@ -57,6 +62,39 @@ void Create3Hmi::OnCreate3Button(const int button) } } +QString Create3Hmi::Namespace() const +{ + return QString::fromStdString(this->namespace_); +} + +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_ << " ' " <create3_button_pub_ = ignition::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->NamespaceChanged(); +} + } // namespace gui } // namespace ignition 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..e8dd0dc0 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,13 @@ #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 "ignition/gui/qt.h" -#include "ignition/gui/Plugin.hh" -#include "ignition/transport/Node.hh" +#include +#include + namespace ignition { @@ -23,6 +24,14 @@ class Create3Hmi : public Plugin { Q_OBJECT + // \brief Name + Q_PROPERTY( + QString name + READ Namespace + WRITE SetNamespace + NOTIFY NamespaceChanged + ) + public: /// \brief Constructor Create3Hmi(); @@ -31,14 +40,30 @@ 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 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 SetNamespace(const QString &_name); + +signals: + /// \brief Notify that robot name has changed + void NamespaceChanged(); 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 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 cf03c0bd..b2dcde64 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 namespace input Label { - id: create3ButtonsLabel - text: "Create3" - font.pixelSize: 22 + id: namespaceLabel + text: "Namespace:" + 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.namespace + placeholderText: qsTr("Robot namespace") + anchors.top: namespaceLabel.bottom + anchors.topMargin: 5 + anchors.left: create3ButtonsRectangle.left + anchors.leftMargin: 10 + onEditingFinished: { + Create3Hmi.SetNamespace(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/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..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,6 +46,7 @@ class PoseRepublisher : public rclcpp::Node tf2::Transform last_dock_pose_; std::string robot_name_; + std::string dock_name_; std::string wheel_joints_[2]; }; 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/interface_buttons/interface_buttons_node.cpp b/irobot_create_ignition/irobot_create_ignition_toolbox/src/interface_buttons/interface_buttons_node.cpp index 62442f82..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)); 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..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,21 +17,23 @@ 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"); + 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, @@ -116,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") { + 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_); 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()); { 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));