diff --git a/README.md b/README.md index fb802cb4..f9de595d 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ Required dependencies: 2. ROS 2 dev tools: - [colcon-common-extensions](https://pypi.org/project/colcon-common-extensions/) - [rosdep](https://pypi.org/project/rosdep/): Used to install dependencies when building from sources - - [vcs](https://pypi.org/project/vcstool/): Automates cloning of git repositories declared on a YAML file. + - [vcs](https://pypi.org/project/vcstool/): Automates cloning of git repositories declared on a YAML file Besides the aforementioned dependencies you will also need at least one among Ignition Gazebo and Classic Gazebo 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 00977a37..0a04b172 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,22 @@ ---- -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 - 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 + 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/offset_parser.py b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/offset_parser.py new file mode 100644 index 00000000..0a9f6ac8 --- /dev/null +++ b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/offset_parser.py @@ -0,0 +1,18 @@ +from launch import LaunchContext, SomeSubstitutionsType, Substitution + + +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}' diff --git a/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/replace_string.py b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/replace_string.py new file mode 100644 index 00000000..9bae46f8 --- /dev/null +++ b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/replace_string.py @@ -0,0 +1,83 @@ +# This file has been copied from https://github.com/ros-planning/navigation2 +# for use as part of the iRobot Create 3 simulator. +# +# Copyright (c) 2019 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import tempfile +from typing import Dict +from typing import List +from typing import Text + +import launch + + +class ReplaceString(launch.Substitution): + """ + Substitution that replaces strings on a given file. + + Used in launch system + """ + + def __init__(self, + source_file: launch.SomeSubstitutionsType, + replacements: Dict) -> None: + super().__init__() + + # import here to avoid loop + from launch.utilities import normalize_to_list_of_substitutions + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__replacements = {} + for key in replacements: + self.__replacements[key] = normalize_to_list_of_substitutions(replacements[key]) + + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' + + def perform(self, context: launch.LaunchContext) -> Text: + output_file = tempfile.NamedTemporaryFile(mode='w', delete=False) + replacements = self.resolve_replacements(context) + try: + input_file = open(launch.utilities.perform_substitutions(context, self.name), 'r') + self.replace(input_file, output_file, replacements) + except Exception as err: # noqa: B902 + print('ReplaceString substitution error: ', err) + finally: + input_file.close() + output_file.close() + return output_file.name + + def resolve_replacements(self, context): + resolved_replacements = {} + for key in self.__replacements: + resolved_replacements[key] = (launch.utilities.perform_substitutions + (context, self.__replacements[key])) + return resolved_replacements + + def replace(self, input_file, output_file, replacements): + for line in input_file: + for key, value in replacements.items(): + if isinstance(key, str) and isinstance(value, str): + if key in line: + line = line.replace(key, value) + else: + raise TypeError('A provided replacement pair is not a string.\ + Both key and value should be strings.') + output_file.write(line) 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..d0547711 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') ] @@ -23,6 +25,9 @@ def generate_launch_description(): # Directories pkg_create3_common_bringup = get_package_share_directory('irobot_create_common_bringup') pkg_create3_control = get_package_share_directory('irobot_create_control') + namespace = LaunchConfiguration('namespace') + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] # Paths control_launch_file = PathJoinSubstitution( @@ -44,87 +49,104 @@ def generate_launch_description(): # Includes diffdrive_controller = IncludeLaunchDescription( - PythonLaunchDescriptionSource([control_launch_file]) + PythonLaunchDescriptionSource([control_launch_file]), + launch_arguments=[('namespace', LaunchConfiguration('namespace'))] ) # Publish hazards vector hazards_vector_node = Node( package='irobot_create_nodes', name='hazards_vector_publisher', + namespace=namespace, executable='hazards_vector_publisher', parameters=[hazards_params_yaml_file, {'use_sim_time': True}], output='screen', + remappings=remappings, ) # Publish IR intensity vector ir_intensity_vector_node = Node( package='irobot_create_nodes', name='ir_intensity_vector_publisher', + namespace=namespace, executable='ir_intensity_vector_publisher', parameters=[ir_intensity_params_yaml_file, {'use_sim_time': True}], output='screen', + remappings=remappings, ) # Motion Control motion_control_node = Node( package='irobot_create_nodes', name='motion_control', + namespace=namespace, executable='motion_control', parameters=[{'use_sim_time': True}], output='screen', + remappings=remappings, ) # Publish wheel status wheel_status_node = Node( package='irobot_create_nodes', name='wheel_status_publisher', + namespace=namespace, executable='wheel_status_publisher', parameters=[wheel_status_params_yaml_file, {'use_sim_time': True}], output='screen', + remappings=remappings, ) # Publish mock topics mock_topics_node = Node( package='irobot_create_nodes', name='mock_publisher', + namespace=namespace, executable='mock_publisher', parameters=[mock_params_yaml_file, {'use_sim_time': True}], output='screen', + remappings=remappings, ) # Publish robot state robot_state_node = Node( package='irobot_create_nodes', name='robot_state', + namespace=namespace, executable='robot_state', parameters=[robot_state_yaml_file, {'use_sim_time': True}], output='screen', + remappings=remappings, ) # Publish kidnap estimator kidnap_estimator_node = Node( package='irobot_create_nodes', name='kidnap_estimator_publisher', + namespace=namespace, executable='kidnap_estimator_publisher', parameters=[kidnap_estimator_yaml_file, {'use_sim_time': True}], output='screen', + remappings=remappings, ) # UI topics / actions ui_mgr_node = Node( package='irobot_create_nodes', name='ui_mgr', + namespace=namespace, executable='ui_mgr', parameters=[ui_mgr_params_yaml_file, {'use_sim_time': True}, {'gazebo': LaunchConfiguration('gazebo')}], output='screen', + remappings=remappings, ) # Define LaunchDescription variable 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..05991ad2 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 @@ -6,6 +6,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.conditions import LaunchConfigurationEquals, LaunchConfigurationNotEquals from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -13,11 +14,15 @@ ARGUMENTS = [ DeclareLaunchArgument('gazebo', default_value='classic', choices=['classic', 'ignition'], - description='Which gazebo simulation to use') + description='Which gazebo simulation to use'), + DeclareLaunchArgument('namespace', default_value='', + description='Create3 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.')) + description=f'{pose_element} ' + + 'component of the dock pose.')) ARGUMENTS.append(DeclareLaunchArgument('visualize_rays', default_value='true', choices=['true', 'false'], @@ -26,7 +31,8 @@ def generate_launch_description(): # Directory - pkg_create3_description = get_package_share_directory('irobot_create_description') + pkg_create3_description = get_package_share_directory( + 'irobot_create_description') # Path dock_xacro_file = PathJoinSubstitution( [pkg_create3_description, 'urdf', 'dock', 'standard_dock.urdf.xacro']) @@ -35,10 +41,15 @@ def generate_launch_description(): x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') yaw = LaunchConfiguration('yaw') visualize_rays = LaunchConfiguration('visualize_rays') - + namespace = LaunchConfiguration('namespace') gazebo_simulator = LaunchConfiguration('gazebo') + frame_prefix = [namespace, '/'] + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('robot_description', 'standard_dock_description')] state_publisher = Node( + condition=LaunchConfigurationEquals('namespace', ''), package='robot_state_publisher', executable='robot_state_publisher', name='dock_state_publisher', @@ -47,16 +58,37 @@ def generate_launch_description(): {'use_sim_time': True}, {'robot_description': Command( - ['xacro', ' ', dock_xacro_file, ' ', - 'gazebo:=', gazebo_simulator, ' ', - 'visualize_rays:=', visualize_rays])}, + ['xacro', ' ', dock_xacro_file, ' ', + 'gazebo:=', gazebo_simulator, ' ', + 'visualize_rays:=', visualize_rays, ' ', + 'namespace:=', namespace, ' '])}, + {'frame_prefix': frame_prefix}, ], - remappings=[ - ('robot_description', 'standard_dock_description'), + remappings=remappings, + ) + + state_publisher_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='robot_state_publisher', + executable='robot_state_publisher', + name='dock_state_publisher', + output='screen', + namespace=namespace, + parameters=[ + {'use_sim_time': True}, + {'robot_description': + Command( + ['xacro', ' ', dock_xacro_file, ' ', + 'gazebo:=', gazebo_simulator, ' ', + 'visualize_rays:=', visualize_rays, ' ', + 'namespace:=', namespace, ' '])}, + {'frame_prefix': frame_prefix}, ], + remappings=remappings, ) tf_odom_std_dock_link_publisher = Node( + condition=LaunchConfigurationEquals('namespace', ''), package='tf2_ros', executable='static_transform_publisher', name='tf_odom_std_dock_link_publisher', @@ -68,10 +100,27 @@ def generate_launch_description(): output='screen', ) + tf_odom_std_dock_link_publisher_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='tf2_ros', + executable='static_transform_publisher', + name='tf_odom_std_dock_link_publisher', + namespace=namespace, + arguments=[x, y, z, + # According to documentation (http://wiki.ros.org/tf2_ros): + # the order is yaw, pitch, roll + yaw, '0', '0', + [namespace, '/odom'], [namespace, '/std_dock_link']], + output='screen', + remappings=remappings, + ) + # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) # Add nodes to LaunchDescription ld.add_action(state_publisher) ld.add_action(tf_odom_std_dock_link_publisher) + ld.add_action(state_publisher_namespaced) + ld.add_action(tf_odom_std_dock_link_publisher_namespaced) return ld 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 06267d09..8ce4f2de 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 @@ -16,7 +16,9 @@ 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='Create3 namespace') ] @@ -25,19 +27,28 @@ 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') + frame_prefix = [namespace, '/'] + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', + namespace=namespace, output='screen', + remappings=remappings, parameters=[ {'use_sim_time': True}, {'robot_description': Command( ['xacro', ' ', xacro_file, ' ', 'gazebo:=', gazebo_simulator, ' ', - 'visualize_rays:=', visualize_rays])}, + 'visualize_rays:=', visualize_rays, ' ', + 'namespace:=', namespace, ' ' + ])}, + {'frame_prefix': frame_prefix}, ], ) @@ -45,7 +56,9 @@ def generate_launch_description(): package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', + namespace=LaunchConfiguration('namespace'), output='screen', + remappings=remappings, ) # 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..d5e3cded 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 @@ -5,18 +5,41 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.actions import DeclareLaunchArgument +from launch.conditions import LaunchConfigurationEquals, LaunchConfigurationNotEquals +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from nav2_common.launch import ReplaceString + +ARGUMENTS = [ + DeclareLaunchArgument('namespace', default_value='', + description='robot namespace') +] def generate_launch_description(): - create_bringup = get_package_share_directory('irobot_create_common_bringup') + create_bringup = get_package_share_directory( + 'irobot_create_common_bringup') + + namespace = LaunchConfiguration('namespace') # Rviz - rviz_config = PathJoinSubstitution([create_bringup, 'rviz', 'irobot_create_view.rviz']) - rviz_logo = PathJoinSubstitution([create_bringup, 'rviz', 'irobot_logo.jpg']) + rviz_config = PathJoinSubstitution( + [create_bringup, 'rviz', 'irobot_create_view.rviz']) + rviz_logo = PathJoinSubstitution( + [create_bringup, 'rviz', 'irobot_logo.jpg']) + + namespaced_rviz_config_file = ReplaceString( + source_file=rviz_config, + replacements={'/robot_description': ('/', namespace, '/robot_description'), + '/standard_dock_description': ('/', namespace, '/standard_dock_description'), + '/initialpose': ('/', namespace, '/initialpose'), + '/clicked_point': ('/', namespace, '/clicked_point'), + '/goal_pose': ('/', namespace, '/goal_pose')} + ) rviz = Node( + condition=LaunchConfigurationEquals('namespace', ''), package='rviz2', executable='rviz2', name='rviz2', @@ -26,9 +49,28 @@ def generate_launch_description(): ] ) + rviz_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=[ + '--display-config', namespaced_rviz_config_file, + '--splash-screen', rviz_logo, + ], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('/goal_pose', 'goal_pose'), + ('/clicked_point', 'clicked_point'), + ('/initialpose', 'initialpose') + ] + ) + # Define LaunchDescription variable ld = LaunchDescription() # Add nodes to LaunchDescription ld.add_action(rviz) + ld.add_action(rviz_namespaced) return ld diff --git a/irobot_create_common/irobot_create_common_bringup/package.xml b/irobot_create_common/irobot_create_common_bringup/package.xml index 921ce761..588dc051 100644 --- a/irobot_create_common/irobot_create_common_bringup/package.xml +++ b/irobot_create_common/irobot_create_common_bringup/package.xml @@ -10,6 +10,7 @@ iRobot ament_cmake + ament_cmake_python irobot_create_control irobot_create_description diff --git a/irobot_create_common/irobot_create_control/config/control.yaml b/irobot_create_common/irobot_create_control/config/control.yaml index 31e64f5f..5fa8dad2 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..2fae73f0 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,40 @@ 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 LaunchConfigurationEquals, 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(): + namespace = LaunchConfiguration('namespace') + namespaced_node_name = [namespace, '/controller_manager'] pkg_create3_control = get_package_share_directory('irobot_create_control') control_params_file = PathJoinSubstitution( [pkg_create3_control, 'config', 'control.yaml']) diffdrive_controller_node = Node( + condition=LaunchConfigurationEquals('namespace', ''), package='controller_manager', executable='spawner', parameters=[control_params_file], - arguments=['diffdrive_controller', '-c', '/controller_manager'], + arguments=['diffdrive_controller', '-c', namespaced_node_name], output='screen', ) joint_state_broadcaster_spawner = Node( + condition=LaunchConfigurationEquals('namespace', ''), package='controller_manager', executable='spawner', - arguments=['joint_state_broadcaster', '-c', '/controller_manager'], + arguments=['joint_state_broadcaster', '-c', namespaced_node_name], output='screen', ) @@ -40,9 +50,39 @@ def generate_launch_description(): ) ) - ld = LaunchDescription() + # Launch with namespace + diffdrive_controller_node_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='controller_manager', + executable='spawner', + namespace=namespace, + parameters=[control_params_file], + arguments=['diffdrive_controller', '-c', namespaced_node_name, '-n', namespace], + output='screen', + ) + + joint_state_broadcaster_spawner_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='controller_manager', + executable='spawner', + namespace=namespace, + arguments=['joint_state_broadcaster', '-c', namespaced_node_name, '-n', namespace], + output='screen', + ) + + # Ensure diffdrive_controller_node starts after joint_state_broadcaster_spawner + diffdrive_controller_callback_namespaced = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner_namespaced, + on_exit=[diffdrive_controller_node_namespaced], + ) + ) + # Define LaunchDescription variable + ld = LaunchDescription(ARGUMENTS) ld.add_action(joint_state_broadcaster_spawner) ld.add_action(diffdrive_controller_callback) + ld.add_action(joint_state_broadcaster_spawner_namespaced) + ld.add_action(diffdrive_controller_callback_namespaced) return ld diff --git a/irobot_create_common/irobot_create_control/package.xml b/irobot_create_common/irobot_create_control/package.xml index c490effd..3bd6f9c8 100644 --- a/irobot_create_common/irobot_create_control/package.xml +++ b/irobot_create_common/irobot_create_control/package.xml @@ -2,7 +2,7 @@ irobot_create_control - 1.0.1 + 1.0.3 Provides the diff-drive controller for the iRobot(R) Create(R) 3 Educational Robot. Ekumen BSD @@ -11,6 +11,9 @@ ament_cmake + gazebo_ros2_control + ign_ros2_control + irobot_create_nodes joint_state_broadcaster ros2launch ros2_controllers diff --git a/irobot_create_common/irobot_create_description/package.xml b/irobot_create_common/irobot_create_description/package.xml index d5da9e77..234b3faf 100644 --- a/irobot_create_common/irobot_create_description/package.xml +++ b/irobot_create_common/irobot_create_description/package.xml @@ -2,7 +2,7 @@ irobot_create_description - 1.0.1 + 1.0.3 Provides the model description for the iRobot(R) Create(R) 3 Educational Robot. Ekumen BSD 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 8cbfbcf6..4abca4f2 100644 --- a/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro +++ b/irobot_create_common/irobot_create_description/urdf/create3.urdf.xacro @@ -55,6 +55,7 @@ + @@ -117,6 +118,9 @@ $(find irobot_create_control)/config/control.yaml + + $(arg namespace) + @@ -125,6 +129,11 @@ $(find irobot_create_control)/config/control.yaml + + $(arg namespace) + /tf:=tf + /tf_static:=tf_static + @@ -235,7 +244,7 @@ - / + $(arg namespace) odom:=sim_ground_truth_pose base_link @@ -251,7 +260,7 @@ - / + $(arg namespace) ~/out:=dock 1.0 diff --git a/irobot_create_common/irobot_create_description/urdf/dock/standard_dock.urdf.xacro b/irobot_create_common/irobot_create_description/urdf/dock/standard_dock.urdf.xacro index cd071e6c..281b4d4e 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_nodes/package.xml b/irobot_create_common/irobot_create_nodes/package.xml index 58c1d193..e4ef209a 100644 --- a/irobot_create_common/irobot_create_nodes/package.xml +++ b/irobot_create_common/irobot_create_nodes/package.xml @@ -2,7 +2,7 @@ irobot_create_nodes - 1.0.1 + 1.0.3 ROS 2 Nodes for the simulated iRobot(R) Create(R) 3 Educational Robot. Ekumen BSD diff --git a/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp b/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp index fe0a142a..c8679241 100644 --- a/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp +++ b/irobot_create_common/irobot_create_nodes/src/motion_control_node.cpp @@ -136,7 +136,6 @@ MotionControlNode::MotionControlNode(const rclcpp::NodeOptions & options) std::bind(&MotionControlNode::set_parameters_callback, this, _1)); auto_override_print_ts_ = this->now(); - current_state_.pose.setIdentity(); last_backup_pose_.setIdentity(); last_teleop_ts_ = this->now(); diff --git a/irobot_create_common/irobot_create_toolbox/package.xml b/irobot_create_common/irobot_create_toolbox/package.xml index 78aba062..33d048bc 100644 --- a/irobot_create_common/irobot_create_toolbox/package.xml +++ b/irobot_create_common/irobot_create_toolbox/package.xml @@ -2,7 +2,7 @@ irobot_create_toolbox - 1.0.1 + 1.0.3 Components and helpers for the iRobot(R) Create(R) 3 Educational Robot. Ekumen BSD 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..8988c479 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 @@ -9,7 +9,10 @@ from pathlib import Path from ament_index_python.packages import get_package_share_directory -from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution + +from irobot_create_common_bringup.offset_parser import OffsetParser + +from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable from launch.conditions import IfCondition @@ -17,24 +20,6 @@ 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}' - - ARGUMENTS = [ DeclareLaunchArgument('use_rviz', default_value='true', choices=['true', 'false'], @@ -47,7 +32,11 @@ def perform( description='Spawn the standard dock model.'), DeclareLaunchArgument('world_path', default_value='', description='Set world path, by default is empty.world'), -] + DeclareLaunchArgument('robot_name', default_value='create3', + description='Robot name'), + DeclareLaunchArgument('namespace', default_value='', + description='robot namespace'), + ] for pose_element in ['x', 'y', 'z', 'yaw']: ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', @@ -76,12 +65,8 @@ def generate_launch_description(): 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']) @@ -95,6 +80,9 @@ def generate_launch_description(): spawn_dock = LaunchConfiguration('spawn_dock') use_gazebo_gui = LaunchConfiguration('use_gazebo_gui') use_rviz = LaunchConfiguration('use_rviz') + robot_name = LaunchConfiguration('robot_name') + namespace = LaunchConfiguration('namespace') + namespaced_robot_description = [namespace, '/robot_description'] # Gazebo server gzserver = ExecuteProcess( @@ -124,6 +112,7 @@ def generate_launch_description(): # 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', @@ -141,28 +130,16 @@ def generate_launch_description(): ) # 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]) - ) + spawn_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(pkg_create3_gazebo_bringup, 'launch', + 'create3_gazebo_spawn.launch.py')), + launch_arguments={'x': x, + 'y': y, + 'z': z, + 'robot_name': robot_name, + 'robot_description': namespaced_robot_description, + 'namespace': namespace, + }.items()) # RVIZ2 rviz2 = IncludeLaunchDescription( @@ -178,12 +155,9 @@ def generate_launch_description(): 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) diff --git a/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_gazebo_spawn.launch.py b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_gazebo_spawn.launch.py new file mode 100644 index 00000000..b75bb3f5 --- /dev/null +++ b/irobot_create_gazebo/irobot_create_gazebo_bringup/launch/create3_gazebo_spawn.launch.py @@ -0,0 +1,83 @@ +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +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'), + DeclareLaunchArgument('robot_name', default_value='create3', + description='Create3 robot name'), + DeclareLaunchArgument('robot_description', default_value='robot_description', + description='robot description topic name'), + 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.')) + + +def generate_launch_description(): + + pkg_create3_common_bringup = get_package_share_directory( + 'irobot_create_common_bringup') + pkg_irobot_create_common_bringup = get_package_share_directory( + 'irobot_create_common_bringup') + + robot_description_launch = PathJoinSubstitution( + [pkg_create3_common_bringup, 'launch', 'robot_description.launch.py']) + create3_nodes_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'create3_nodes.launch.py']) + robot_description_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'robot_description.launch.py']) + + # Launch configurations + x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') + yaw = LaunchConfiguration('yaw') + robot_name = LaunchConfiguration('robot_name') + robot_description = LaunchConfiguration('robot_description') + namespace = LaunchConfiguration('namespace') + gazebo = LaunchConfiguration('gazebo') + + # Spawn robot + spawn_robot = 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', + ) + + # Robot description + robot_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([robot_description_launch]), + launch_arguments={'gazebo': gazebo, 'namespace': namespace}.items()) + + # Create3 nodes + create3_nodes = IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_nodes_launch]), + launch_arguments=[('namespace', namespace)]) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(spawn_robot) + ld.add_action(robot_description_launch) + # Include Create 3 nodes + ld.add_action(create3_nodes) + return ld diff --git a/irobot_create_gazebo/irobot_create_gazebo_bringup/package.xml b/irobot_create_gazebo/irobot_create_gazebo_bringup/package.xml index 53bd1860..c54dc835 100644 --- a/irobot_create_gazebo/irobot_create_gazebo_bringup/package.xml +++ b/irobot_create_gazebo/irobot_create_gazebo_bringup/package.xml @@ -2,7 +2,7 @@ irobot_create_gazebo_bringup - 1.0.1 + 1.0.3 Provides launch and configuration scripts for a Gazebo simulated iRobot(R) Create(R) 3 Educational Robot. Ekumen BSD diff --git a/irobot_create_gazebo/irobot_create_gazebo_plugins/package.xml b/irobot_create_gazebo/irobot_create_gazebo_plugins/package.xml index 92c0c393..d2c7a74c 100644 --- a/irobot_create_gazebo/irobot_create_gazebo_plugins/package.xml +++ b/irobot_create_gazebo/irobot_create_gazebo_plugins/package.xml @@ -2,7 +2,7 @@ irobot_create_gazebo_plugins - 1.0.1 + 1.0.3 Provides the Gazebo plugins for the iRobot(R) Create(R) 3 Educational Robot. Ekumen BSD diff --git a/irobot_create_gazebo/irobot_create_gazebo_sim/package.xml b/irobot_create_gazebo/irobot_create_gazebo_sim/package.xml index a61e6c4c..4caa465e 100644 --- a/irobot_create_gazebo/irobot_create_gazebo_sim/package.xml +++ b/irobot_create_gazebo/irobot_create_gazebo_sim/package.xml @@ -2,7 +2,7 @@ irobot_create_gazebo_sim - 1.0.1 + 1.0.3 Metapackage for the iRobot(R) Create(R) 3 robot Gazebo simulatorGazebo simulation stack. Ekumen 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/launch/create3_ignition.launch.py b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ignition.launch.py index 057c34b3..491e3bba 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 @@ -7,31 +7,11 @@ 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.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', @@ -49,8 +29,9 @@ def perform( 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.')) @@ -59,8 +40,6 @@ 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( @@ -87,22 +66,13 @@ def generate_launch_description(): # Paths ign_gazebo_launch = PathJoinSubstitution( [pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py']) - ros_ign_bridge_launch = PathJoinSubstitution( - [pkg_irobot_create_ignition_bringup, 'launch', 'create3_ros_ignition_bridge.launch.py']) - create3_nodes_launch = PathJoinSubstitution( - [pkg_irobot_create_common_bringup, 'launch', 'create3_nodes.launch.py']) - create3_ignition_nodes_launch = PathJoinSubstitution( - [pkg_irobot_create_ignition_bringup, 'launch', 'create3_ignition_nodes.launch.py']) - robot_description_launch = PathJoinSubstitution( - [pkg_irobot_create_common_bringup, 'launch', 'robot_description.launch.py']) - dock_description_launch = PathJoinSubstitution( - [pkg_irobot_create_common_bringup, 'launch', 'dock_description.launch.py']) - rviz2_launch = PathJoinSubstitution( - [pkg_irobot_create_common_bringup, 'launch', 'rviz2.launch.py']) # Launch configurations x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') yaw = LaunchConfiguration('yaw') + robot_name = LaunchConfiguration('robot_name') + world = LaunchConfiguration('world') + namespace = LaunchConfiguration('namespace') # Ignition gazebo ignition_gazebo = IncludeLaunchDescription( @@ -117,61 +87,21 @@ def generate_launch_description(): ] ) - 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'))] + spawn_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(pkg_irobot_create_ignition_bringup, 'launch', + 'create3_spawn.launch.py')), + launch_arguments={'x': x, + 'y': y, + 'z': z, + 'yaw': yaw, + 'robot_name': robot_name, + 'robot_description': '/robot_description', + 'world': world, + 'use_rviz': LaunchConfiguration('use_rviz'), + 'namespace': namespace, + 'spawn_dock': LaunchConfiguration('spawn_dock'), + }.items() ) # Create launch description and add actions @@ -179,12 +109,6 @@ def generate_launch_description(): 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) + 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..6114cfc9 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 @@ -2,15 +2,17 @@ # @author Roni Kreinin (rkreinin@clearpathrobotics.com) from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node + ARGUMENTS = [ DeclareLaunchArgument('robot_name', default_value='create3', - description='Robot name') + description='Robot name'), + DeclareLaunchArgument('namespace', default_value='', + description='Robot namespace') ] @@ -24,10 +26,13 @@ def generate_launch_description(): sensors_params_yaml_file = PathJoinSubstitution( [pkg_create3_ignition_bringup, 'config', 'sensors_params.yaml']) + namespace = LaunchConfiguration('namespace') + # Pose republisher pose_republisher_node = Node( package='irobot_create_ignition_toolbox', name='pose_republisher_node', + namespace=namespace, executable='pose_republisher_node', parameters=[pose_republisher_params_yaml_file, {'robot_name': LaunchConfiguration('robot_name')}, @@ -39,6 +44,7 @@ def generate_launch_description(): sensors_node = Node( package='irobot_create_ignition_toolbox', name='sensors_node', + namespace=namespace, executable='sensors_node', parameters=[sensors_params_yaml_file, {'use_sim_time': True}], @@ -49,6 +55,7 @@ def generate_launch_description(): interface_buttons_node = Node( package='irobot_create_ignition_toolbox', name='interface_buttons_node', + namespace=namespace, executable='interface_buttons_node', parameters=[{'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..20cdac28 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_ros_ignition_bridge.launch.py @@ -3,7 +3,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition +from launch.conditions import IfCondition, LaunchConfigurationEquals, LaunchConfigurationNotEquals from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -15,13 +15,17 @@ DeclareLaunchArgument('robot_name', default_value='create3', description='Ignition model name'), DeclareLaunchArgument('world', default_value='depot', - description='World name') + description='World name'), + DeclareLaunchArgument('namespace', default_value='', + description='Robot namespace') ] def generate_launch_description(): - namespace = LaunchConfiguration('robot_name') + namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') + robot_name = LaunchConfiguration('robot_name') + world = LaunchConfiguration('world') cliff_sensors = [ 'cliff_front_left', @@ -41,7 +45,9 @@ def generate_launch_description(): ] # clock bridge - clock_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', + clock_bridge = Node( + package='ros_ign_bridge', + executable='parameter_bridge', namespace=namespace, name='clock_bridge', output='screen', @@ -51,25 +57,49 @@ def generate_launch_description(): condition=IfCondition(use_sim_time)) # cmd_vel bridge - cmd_vel_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', + cmd_vel_bridge = Node( + condition=LaunchConfigurationEquals('namespace', ''), + 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') - ]) + 'use_sim_time': use_sim_time}], + arguments=[['/cmd_vel' + + '@geometry_msgs/msg/Twist' + + '[ignition.msgs.Twist'], + ['/model/', robot_name, '/cmd_vel' + + '@geometry_msgs/msg/Twist' + + ']ignition.msgs.Twist']], + remappings=[(['/model/', + robot_name, '/cmd_vel'], + ['/diffdrive_controller/cmd_vel_unstamped'])]) + + cmd_vel_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='ros_ign_bridge', + executable='parameter_bridge', + name='cmd_vel_bridge', + namespace=namespace, + 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=[(['/model/', + robot_name, '/cmd_vel'], + ['/', namespace, + '/diffdrive_controller/cmd_vel_unstamped'])]) # Pose bridge - pose_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', + pose_bridge = Node( + condition=LaunchConfigurationEquals('namespace', ''), + package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, name='pose_bridge', output='screen', @@ -77,22 +107,47 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), '/pose' + + ['/model/', robot_name, '/pose' + '@tf2_msgs/msg/TFMessage' + '[ignition.msgs.Pose_V'], - '/model/standard_dock/pose' + - '@tf2_msgs/msg/TFMessage' + - '[ignition.msgs.Pose_V' + ['/model/', robot_name, '_standard_dock', '/pose' + + '@tf2_msgs/msg/TFMessage' + + '[ignition.msgs.Pose_V'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/pose'], + (['/model/', robot_name, '/pose'], '/_internal/sim_ground_truth_pose'), - ('/model/standard_dock/pose', - '/_internal/sim_ground_truth_dock_pose') - ]) + (['/model/', robot_name, '_standard_dock', '/pose'], + '/_internal/sim_ground_truth_dock_pose')]) + + pose_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='ros_ign_bridge', executable='parameter_bridge', + namespace=namespace, + name='pose_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[ + ['/model/', robot_name, '/pose' + + '@tf2_msgs/msg/TFMessage' + + '[ignition.msgs.Pose_V'], + ['/model/', robot_name, '_standard_dock', '/pose' + + '@tf2_msgs/msg/TFMessage' + + '[ignition.msgs.Pose_V'] + ], + remappings=[ + (['/model/', robot_name, '/pose'], + ['/', namespace, '/_internal/sim_ground_truth_pose']), + (['/model/', robot_name, '_standard_dock', '/pose'], + ['/', namespace, '/_internal/sim_ground_truth_dock_pose'])]) # odom to base_link transform bridge - odom_base_tf_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', + odom_base_tf_bridge = Node( + condition=LaunchConfigurationEquals('namespace', ''), + package='ros_ign_bridge', + executable='parameter_bridge', namespace=namespace, name='odom_base_tf_bridge', output='screen', @@ -100,16 +155,35 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), '/tf' + + ['/model/', robot_name, '/tf' + '@tf2_msgs/msg/TFMessage' + '[ignition.msgs.Pose_V'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/tf'], '/tf') - ]) + (['/model/', robot_name, '/tf'], '/tf')]) + + odom_base_tf_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='ros_ign_bridge', + executable='parameter_bridge', + namespace=namespace, + name='odom_base_tf_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[ + ['/model/', robot_name, '/tf' + + '@tf2_msgs/msg/TFMessage' + + '[ignition.msgs.Pose_V'] + ], + remappings=[ + (['/model/', robot_name, '/tf'], ['/', namespace, '/tf'])]) # Bumper contact sensor bridge - bumper_contact_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', + bumper_contact_bridge = Node( + condition=LaunchConfigurationEquals('namespace', ''), + package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, name='bumper_contact_bridge', output='screen', @@ -117,19 +191,42 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), + ['/model/', robot_name, '/bumper_contact' + '@ros_ign_interfaces/msg/Contacts' + '[ignition.msgs.Contacts'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), + (['/model/', robot_name, '/bumper_contact'], - '/bumper_contact') - ]) + '/bumper_contact')]) + + bumper_contact_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', + ''), + namespace=namespace, + package='ros_ign_bridge', + executable='parameter_bridge', + name='bumper_contact_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[ + ['/model/', robot_name, + '/bumper_contact' + + '@ros_ign_interfaces/msg/Contacts' + + '[ignition.msgs.Contacts'] + ], + remappings=[ + (['/model/', robot_name, + '/bumper_contact'], + ['/', namespace, '/bumper_contact'])]) # Cliff bridge - cliff_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', + cliff_bridge = Node( + condition=LaunchConfigurationEquals('namespace', ''), + package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, name='cliff_bridge', output='screen', @@ -137,21 +234,46 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', 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'), + (['/world/', world, + '/model/', robot_name, '/link/base_link/sensor/' + cliff + '/scan'], '/_internal/' + cliff + '/scan') - for cliff in cliff_sensors - ]) + for cliff in cliff_sensors]) + + cliff_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='ros_ign_bridge', executable='parameter_bridge', + namespace=namespace, + name='cliff_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[ + ['/world/', world, + '/model/', robot_name, + '/link/base_link/sensor/' + cliff + '/scan' + + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + for cliff in cliff_sensors + ], + remappings=[ + (['/world/', world, + '/model/', robot_name, + '/link/base_link/sensor/' + cliff + '/scan'], + ['/', namespace, '/_internal/', cliff, '/scan']) + for cliff in cliff_sensors]) + # IR intensity bridge - ir_intensity_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', + ir_intensity_bridge = Node( + condition=LaunchConfigurationEquals('namespace', ''), + package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, name='ir_intensity_bridge', output='screen', @@ -159,41 +281,93 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', 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 - ]) + (['/world/', world, + '/model/', robot_name, + '/link/' + ir + '/sensor/' + ir + '/scan'], + '/_internal/' + ir + '/scan') for ir in ir_intensity_sensors]) + + ir_intensity_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='ros_ign_bridge', executable='parameter_bridge', + namespace=namespace, + name='ir_intensity_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[ + ['/world/', world, + '/model/', robot_name, + '/link/' + ir + '/sensor/' + ir + '/scan' + + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan' + ] + for ir in ir_intensity_sensors + ], + remappings=[ + (['/world/', world, + '/model/', robot_name, + '/link/' + ir + '/sensor/' + ir + '/scan'], + ['/', namespace, '/_internal/', ir, '/scan']) + for ir in ir_intensity_sensors]) # Buttons message bridge - buttons_msg_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', + buttons_msg_bridge = Node( + condition=LaunchConfigurationEquals('namespace', ''), + package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, name='buttons_msg_bridge', output='screen', parameters=[{ - 'use_sim_time': use_sim_time + 'use_sim_time': use_sim_time }], arguments=[ - ['/create3/buttons' + - '@std_msgs/msg/Int32' + - '[ignition.msgs.Int32'] - ]) + ['/model/', robot_name, '/buttons' + + '@std_msgs/msg/Int32' + + '[ignition.msgs.Int32'] + ], + remappings=[ + (['/model/', robot_name, '/buttons'], + '/buttons')]) + + buttons_msg_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='ros_ign_bridge', executable='parameter_bridge', + namespace=namespace, + name='buttons_msg_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time}], + arguments=[ + ['/model/', robot_name, '/buttons' + + '@std_msgs/msg/Int32' + + '[ignition.msgs.Int32'] + ], + remappings=[ + (['/model/', robot_name, '/buttons'], + ['/', namespace, '/buttons'])]) # Create launch description and add actions ld = LaunchDescription(ARGUMENTS) ld.add_action(clock_bridge) ld.add_action(cmd_vel_bridge) + ld.add_action(cmd_vel_bridge_namespaced) ld.add_action(pose_bridge) + ld.add_action(pose_bridge_namespaced) ld.add_action(odom_base_tf_bridge) + ld.add_action(odom_base_tf_bridge_namespaced) ld.add_action(bumper_contact_bridge) + ld.add_action(bumper_contact_bridge_namespaced) ld.add_action(cliff_bridge) + ld.add_action(cliff_bridge_namespaced) ld.add_action(ir_intensity_bridge) + ld.add_action(ir_intensity_bridge_namespaced) ld.add_action(buttons_msg_bridge) + ld.add_action(buttons_msg_bridge_namespaced) 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..319d1365 --- /dev/null +++ b/irobot_create_ignition/irobot_create_ignition_bringup/launch/create3_spawn.launch.py @@ -0,0 +1,164 @@ +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution +from launch.actions import DeclareLaunchArgument, 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 + + +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('gazebo', default_value='ignition', + choices=['classic', 'ignition'], + description='Which gazebo simulator to use'), + DeclareLaunchArgument('visualize_rays', default_value='false', + choices=['true', 'false'], + description='Enable/disable ray visualization'), + DeclareLaunchArgument('world', default_value='depot', + description='World name'), + DeclareLaunchArgument('robot_name', default_value='create3', + description='Create3 robot name'), + DeclareLaunchArgument('robot_description', default_value='robot_description', + description='robot description topic 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.'), +] + +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(): + + pkg_create3_common_bringup = get_package_share_directory( + 'irobot_create_common_bringup') + 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') + + robot_description_launch = PathJoinSubstitution( + [pkg_create3_common_bringup, 'launch', 'robot_description.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']) + rviz2_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'rviz2.launch.py']) + dock_description_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'dock_description.launch.py']) + + # Launch configurations + x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') + yaw = LaunchConfiguration('yaw') + robot_name = LaunchConfiguration('robot_name') + namespace = LaunchConfiguration('namespace') + namespaced_robot_description = [namespace, '/robot_description'] + namespaced_dock_description = [namespace, '/standard_dock_description'] + + # Robot description + robot_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([robot_description_launch]), + launch_arguments={'gazebo': 'ignition', 'namespace': namespace}.items()) + + # Dock description + 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, + 'namespace': namespace, + 'gazebo': 'ignition'}.items() + ) + + rviz2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz2_launch]), + condition=IfCondition(LaunchConfiguration('use_rviz')), + launch_arguments={'namespace': namespace}.items() + ) + + # Spawn robot + spawn_robot = Node( + package='ros_ign_gazebo', + executable='create', + output='screen', + arguments=[ + '-name', robot_name, + '-topic', namespaced_robot_description, + '-Y', yaw, + '-x', x, + '-y', y, + '-z', z]) + + # Dock + spawn_dock = Node(package='ros_ign_gazebo', + executable='create', + output='screen', + arguments=['-name', (robot_name, '_standard_dock'), + '-x', x_dock, + '-y', y, + '-z', z, + '-Y', yaw_dock, + '-topic', namespaced_dock_description], + ) + + # ROS Ign bridge + ros_ign_bridge = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ros_ign_bridge_launch]), + launch_arguments=[('world', LaunchConfiguration('world')), + ('robot_name', robot_name), + ('namespace', namespace)] + ) + + # Create3 nodes + create3_nodes = IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_nodes_launch]), + launch_arguments=[('namespace', namespace)] + ) + + create3_ignition_nodes = IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), + launch_arguments=[('robot_name', LaunchConfiguration('robot_name')), + ('namespace', namespace)] + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(spawn_robot) + ld.add_action(spawn_dock) + ld.add_action(dock_description) + ld.add_action(ros_ign_bridge) + ld.add_action(rviz2) + ld.add_action(robot_description_launch) + ld.add_action(create3_nodes) + ld.add_action(create3_ignition_nodes) + return ld diff --git a/irobot_create_ignition/irobot_create_ignition_bringup/package.xml b/irobot_create_ignition/irobot_create_ignition_bringup/package.xml index d44ab6b6..0501b803 100644 --- a/irobot_create_ignition/irobot_create_ignition_bringup/package.xml +++ b/irobot_create_ignition/irobot_create_ignition_bringup/package.xml @@ -2,7 +2,7 @@ irobot_create_ignition_bringup - 1.0.1 + 1.0.3 Provides launch and configuration scripts for a Ignition simulated iRobot(R) Create(R) 3 Educational Robot. rkreinin BSD 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 ca518f79..34e1b526 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.cc @@ -32,14 +32,17 @@ Create3Hmi::~Create3Hmi() void Create3Hmi::LoadConfig(const tinyxml2::XMLElement * _pluginElem) { - if (!_pluginElem) { - return; - } - if (this->title.empty()) { this->title = "Create3 HMI"; } + if (_pluginElem) + { + auto nameElem = _pluginElem->FirstChildElement("name"); + if (nullptr != nameElem && nullptr != nameElem->GetText()) + this->SetName(nameElem->GetText()); + } + this->connect( this, SIGNAL(AddMsg(QString)), this, SLOT(OnAddMsg(QString)), Qt::QueuedConnection); @@ -57,6 +60,38 @@ void Create3Hmi::OnCreate3Button(const int button) } } +QString Create3Hmi::Name() const +{ + return QString::fromStdString(this->robot_name_); +} + +void Create3Hmi::SetName(const QString &_name) +{ + this->robot_name_ = _name.toStdString(); + this->create3_button_topic_ = "/model/" + this->robot_name_ + "/buttons"; + ignmsg << "A new robot name has been entered, publishing on topic: '" << + this->create3_button_topic_ << " ' " <create3_button_pub_ = gz::transport::Node::Publisher(); + this->create3_button_pub_ = + this->node_.Advertise< ignition::msgs::Int32 > + (this->create3_button_topic_); + if (!this->create3_button_pub_) + { + App()->findChild()->notifyWithDuration( + QString::fromStdString("Error when advertising topic: " + + this->create3_button_topic_), 4000); + ignerr << "Error when advertising topic: " << + this->create3_button_topic_ << std::endl; + }else { + App()->findChild()->notifyWithDuration( + QString::fromStdString("Advertising topic: '" + + this->create3_button_topic_ + "'"), 4000); + } + this->NameChanged(); +} + } // namespace gui } // namespace ignition 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 11ad1504..d4481e8a 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.hh @@ -23,6 +23,14 @@ class Create3Hmi : public Plugin { Q_OBJECT + // \brief Name + Q_PROPERTY( + QString name + READ Name + WRITE SetName + NOTIFY NameChanged + ) + public: /// \brief Constructor Create3Hmi(); @@ -31,14 +39,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 Name() const; + +public slots: + /// \brief Callback in Qt thread when the robot name changes. + /// \param[in] _name variable to indicate the robot name to + /// publish the Button commands. + void SetName(const QString &_name); + +signals: + /// \brief Notify that robot name has changed + void NameChanged(); protected slots: /// \brief Callback trigged when the button is pressed. void OnCreate3Button(const int button); + private: ignition::transport::Node node_; ignition::transport::Node::Publisher create3_button_pub_; - std::string create3_button_topic_ = "/create3/buttons"; + std::string robot_name_ = "create3"; + std::string create3_button_topic_ = "/model/create3/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..58838f54 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml +++ b/irobot_create_ignition/irobot_create_ignition_plugins/Create3Hmi/Create3Hmi.qml @@ -17,30 +17,47 @@ Rectangle anchors.fill: parent focus: true Layout.minimumWidth: 400 - Layout.minimumHeight: 175 + Layout.minimumHeight: 225 Rectangle { id: create3ButtonsRectangle - border.color: "black" border.width: 2 anchors.top: widgetRectangle.top anchors.left: widgetRectangle.left focus: true - height: 125 + height: 175 width: 400 - // Buttons + // Robot Name input Label { - id: create3ButtonsLabel - text: "Create3" - font.pixelSize: 22 + id: nameLabel + text: "Robot Name:" + Layout.fillWidth: true + Layout.margins: 10 anchors.top: create3ButtonsRectangle.top anchors.topMargin: 10 - anchors.left: parent.left + anchors.left: create3ButtonsRectangle.left anchors.leftMargin: 10 } + TextField { + id: nameField + width: 175 + Layout.fillWidth: true + Layout.margins: 10 + text: Create3Hmi.name + placeholderText: qsTr("input robot name") + anchors.top: nameLabel.bottom + anchors.topMargin: 5 + anchors.left: create3ButtonsRectangle.left + anchors.leftMargin: 10 + onEditingFinished: { + Create3Hmi.SetName(text) + } + } + + // Button inputs ToolButton { id: create3Button1 anchors.verticalCenter: create3ButtonPower.verticalCenter @@ -62,8 +79,8 @@ Rectangle ToolButton { id: create3ButtonPower - anchors.top: create3ButtonsLabel.bottom - anchors.topMargin: 0 + anchors.bottom: create3ButtonsRectangle.bottom + anchors.bottomMargin: 15 anchors.horizontalCenter: create3ButtonsRectangle.horizontalCenter checkable: true checked: true diff --git a/irobot_create_ignition/irobot_create_ignition_plugins/package.xml b/irobot_create_ignition/irobot_create_ignition_plugins/package.xml index c1511fdf..37331666 100644 --- a/irobot_create_ignition/irobot_create_ignition_plugins/package.xml +++ b/irobot_create_ignition/irobot_create_ignition_plugins/package.xml @@ -2,7 +2,7 @@ irobot_create_ignition_plugins - 1.0.1 + 1.0.3 Ignition plugins for simulated iRobot(R) Create(R) 3 Educational Robot. rkreinin BSD diff --git a/irobot_create_ignition/irobot_create_ignition_sim/package.xml b/irobot_create_ignition/irobot_create_ignition_sim/package.xml index ce7f7d08..7eda7b41 100644 --- a/irobot_create_ignition/irobot_create_ignition_sim/package.xml +++ b/irobot_create_ignition/irobot_create_ignition_sim/package.xml @@ -2,7 +2,7 @@ irobot_create_ignition_sim - 1.0.1 + 1.0.3 Metapackage for the iRobot(R) Create(R) 3 robot Ignition simulator rkreinin diff --git a/irobot_create_ignition/irobot_create_ignition_toolbox/package.xml b/irobot_create_ignition/irobot_create_ignition_toolbox/package.xml index 07b85ae7..b947816c 100644 --- a/irobot_create_ignition/irobot_create_ignition_toolbox/package.xml +++ b/irobot_create_ignition/irobot_create_ignition_toolbox/package.xml @@ -2,7 +2,7 @@ irobot_create_ignition_toolbox - 1.0.1 + 1.0.3 Nodes and tools for simulating in Ignition iRobot(R) Create(R) 3 Educational Robot. rkreinin BSD 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..433020d5 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 @@ -8,11 +8,13 @@ using irobot_create_ignition_toolbox::InterfaceButtons; + InterfaceButtons::InterfaceButtons() : rclcpp::Node("sensors_node") { + // TODO(sophia) get robot_name instead of unsing namespace by default interface_buttons_sub_ = this->create_subscription( - "create3/buttons", + "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..7f73a3ed 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 @@ -116,12 +116,10 @@ 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") { - auto odom_msg = utils::tf_message_to_odom(msg, i); - // Save dock pose - tf2::convert(odom_msg->pose.pose, last_dock_pose_); - dock_publisher_->publish(std::move(odom_msg)); - } else if (msg->transforms[i].child_frame_id.find("halo_link") != std::string::npos) { + std::string frame_id = msg->transforms[i].child_frame_id; + bool has_trailling_slash = frame_id.find("standard_dock/") == std::string::npos; + bool is_standard_dock = frame_id.find("standard_dock") != std::string::npos; + if (frame_id.find("halo_link") != std::string::npos) { // Send IR Opcode Emitter transform auto emitter_msg = utils::tf_message_to_odom(msg, i); tf2::Transform emitter_pose; @@ -134,6 +132,11 @@ void PoseRepublisher::dock_subscriber_callback(const tf2_msgs::msg::TFMessage::S utils::tf2_transform_to_pose(emitter_world_pose, emitter_msg->pose.pose); // Publish ir_opcode_emitter_publisher_->publish(std::move(emitter_msg)); + } else if (has_trailling_slash && is_standard_dock) { + auto odom_msg = utils::tf_message_to_odom(msg, i); + // Save dock pose + tf2::convert(odom_msg->pose.pose, last_dock_pose_); + dock_publisher_->publish(std::move(odom_msg)); } } } 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 18eaed2c..38a1a88d 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)); @@ -257,6 +257,7 @@ void IrOpcode::PublishSensors(const std::array detected_opcodes) // First for sensor 0 then sensor 1 for (size_t k = 0; k < detected_opcodes.size(); k++) { const int detected_opcode = detected_opcodes[k]; + if (detected_opcode > 0) { // Fill msg for this iteration auto msg_ = irobot_create_msgs::msg::IrOpcode(); 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));