diff --git a/mir_description/CMakeLists.txt b/mir_description/CMakeLists.txt index 1f40d356..eebdd78f 100644 --- a/mir_description/CMakeLists.txt +++ b/mir_description/CMakeLists.txt @@ -19,7 +19,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(xacro REQUIRED) find_package(robot_state_publisher REQUIRED) -find_package(gazebo_ros REQUIRED) +find_package(ros_gz_sim REQUIRED) find_package(rviz2 REQUIRED) find_package(urdf REQUIRED) find_package(xacro REQUIRED) diff --git a/mir_description/launch/robot_state_publisher.launch b/mir_description/launch/robot_state_publisher.launch index def45f94..a40c5e4a 100644 --- a/mir_description/launch/robot_state_publisher.launch +++ b/mir_description/launch/robot_state_publisher.launch @@ -3,7 +3,7 @@ - + diff --git a/mir_description/package.xml b/mir_description/package.xml index f0f06525..44b7fa39 100644 --- a/mir_description/package.xml +++ b/mir_description/package.xml @@ -15,10 +15,12 @@ ament_cmake + ros_gz_interfaces + ros_gz_sim + joint_state_publisher joint_state_publisher_gui robot_state_publisher - gazebo_ros rviz2 urdf xacro diff --git a/mir_description/urdf/include/common.gazebo.xacro b/mir_description/urdf/include/common.gazebo.xacro deleted file mode 100644 index 71f83b46..00000000 --- a/mir_description/urdf/include/common.gazebo.xacro +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - robot_description - robot_state_publisher - - ${robot_namespace} - - 0.001 - - - - diff --git a/mir_description/urdf/include/common_properties.urdf.xacro b/mir_description/urdf/include/common_properties.urdf.xacro index 3c1a28df..1e022fd5 100644 --- a/mir_description/urdf/include/common_properties.urdf.xacro +++ b/mir_description/urdf/include/common_properties.urdf.xacro @@ -2,8 +2,9 @@ - - + + + diff --git a/mir_description/urdf/include/imu.gazebo.urdf.xacro b/mir_description/urdf/include/imu.gazebo.urdf.xacro deleted file mode 100644 index 424972c4..00000000 --- a/mir_description/urdf/include/imu.gazebo.urdf.xacro +++ /dev/null @@ -1,86 +0,0 @@ - - - - - - - - - - - - - - - - true - - true - ${update_rate} - true - - - - - 0.0 - ${stdev} - 0.0000075 - 0.0000008 - - - - - 0.0 - ${stdev} - 0.0000075 - 0.0000008 - - - - - 0.0 - ${stdev} - 0.0000075 - 0.0000008 - - - - - - - 0.0 - ${stdev} - 0.1 - 0.001 - - - - - 0.0 - ${stdev} - 0.1 - 0.001 - - - - - 0.0 - ${stdev} - 0.1 - 0.001 - - - - - - false - - ${imu_topic} - ~/out:=data - ${imu_frame} - - - - - - diff --git a/mir_description/urdf/include/mir.gazebo.xacro b/mir_description/urdf/include/mir.gazebo.xacro index 374f7f19..e69ffba6 100644 --- a/mir_description/urdf/include/mir.gazebo.xacro +++ b/mir_description/urdf/include/mir.gazebo.xacro @@ -1,42 +1,85 @@ - + - - + - - - - ${ns} - - 1000.0 - ${right_wheel_joint} - ${left_wheel_joint} - + + + + - - - cmd_vel:=${prefix}cmd_vel - odom:=${prefix}odom - + + + + + ${prefix}${topic} + ${prefix}${left_wheel_joint} + ${prefix}${right_wheel_joint} + + + + - 1000.0 + + + + + + ${left_wheel_joint} ${right_wheel_joint} + + ${wheel_separation} - ${2*wheel_radius} - ${prefix}odom + ${wheel_radius} + + + 2.8 + + + + + ${prefix}cmd_vel + ${prefix}odom + ${prefix}odom + ${prefix}base_footprint + 100 + /tf @@ -61,7 +104,11 @@ - + + + + + + + + + + + 0 0 0 0 0 0 + ${prefix}${link} + ${update_rate} + ${prefix}${imu_topic} + 1 + true + + + + + + 0.0 + ${stdev} + 0.0000075 + 0.0000008 + + + + + 0.0 + ${stdev} + 0.0000075 + 0.0000008 + + + + + 0.0 + ${stdev} + 0.0000075 + 0.0000008 + + + + + + + + 0.0 + ${stdev} + 0.1 + 0.001 + + + + + 0.0 + ${stdev} + 0.1 + 0.001 + + + + + 0.0 + ${stdev} + 0.1 + 0.001 + + + + + + + diff --git a/mir_description/urdf/include/mir.transmission.xacro b/mir_description/urdf/include/mir.transmission.xacro index 0fe6ae0a..0c7d1273 100644 --- a/mir_description/urdf/include/mir.transmission.xacro +++ b/mir_description/urdf/include/mir.transmission.xacro @@ -1,5 +1,5 @@ - + diff --git a/mir_description/urdf/include/mir_100_v1.urdf.xacro b/mir_description/urdf/include/mir_100_v1.urdf.xacro new file mode 100644 index 00000000..2b606c76 --- /dev/null +++ b/mir_description/urdf/include/mir_100_v1.urdf.xacro @@ -0,0 +1,12 @@ + + + + + + + + + + + diff --git a/mir_description/urdf/include/mir_v1.urdf.xacro b/mir_description/urdf/include/mir_v1.urdf.xacro index 224ff112..edf8e417 100644 --- a/mir_description/urdf/include/mir_v1.urdf.xacro +++ b/mir_description/urdf/include/mir_v1.urdf.xacro @@ -1,10 +1,9 @@ - + - @@ -64,12 +63,9 @@ - - Gazebo/FlatBlack - - + @@ -79,16 +75,6 @@ - - - - ${ns} - - 200.0 - ${prefix}${locationprefix}_caster_rotation_joint - - - @@ -116,9 +102,6 @@ - - Gazebo/Grey - @@ -128,16 +111,6 @@ - - - - ${ns} - - 200.0 - ${prefix}${locationprefix}_caster_wheel_joint - - - @@ -156,12 +129,9 @@ - - Gazebo/FlatBlack - - + @@ -188,9 +158,6 @@ - - Gazebo/White - @@ -201,8 +168,6 @@ - - @@ -259,10 +224,10 @@ - - - - + + + + @@ -278,6 +243,32 @@ - + + + + + + + + + + + + + + + + diff --git a/mir_description/urdf/include/sick_s300.urdf.xacro b/mir_description/urdf/include/sick_s300.urdf.xacro index f2d4be3b..58fb9933 100644 --- a/mir_description/urdf/include/sick_s300.urdf.xacro +++ b/mir_description/urdf/include/sick_s300.urdf.xacro @@ -1,5 +1,5 @@ - + @@ -30,20 +30,24 @@ - - - - - false + + 0 0 0 0 0 0 + ${prefix}${topic} 12.5 541 1 - -2.35619449615 - 2.35619449615 + -2.26893 + 2.26893 + + 1 + 0.01 + 0 + 0 + 0.05 @@ -59,13 +63,9 @@ 0.01 - - - ~/out:=${prefix}${topic} - - sensor_msgs/LaserScan - ${prefix}${link} - + 1 + false + ${prefix}${link} diff --git a/mir_description/urdf/mir.urdf.xacro b/mir_description/urdf/mir.urdf.xacro index 0dcd3c4f..05ae7027 100644 --- a/mir_description/urdf/mir.urdf.xacro +++ b/mir_description/urdf/mir.urdf.xacro @@ -1,28 +1,19 @@ - + - - - - - - + + - - + + - + - diff --git a/mir_gazebo/CMakeLists.txt b/mir_gazebo/CMakeLists.txt index cbb284d0..b99ed04d 100644 --- a/mir_gazebo/CMakeLists.txt +++ b/mir_gazebo/CMakeLists.txt @@ -1,4 +1,5 @@ cmake_minimum_required(VERSION 3.5.1) +set(CMAKE_POLICY_DEFAULT_CMP0144 NEW) # to remove warning "Policy CMP0144 is not set" project(mir_gazebo) # Default to C99 @@ -18,13 +19,11 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(mir_description REQUIRED) -find_package(gazebo REQUIRED) -find_package(gazebo_ros REQUIRED) -find_package(gazebo_ros_pkgs REQUIRED) +find_package(ros_gz_sim REQUIRED) find_package(ira_laser_tools REQUIRED) install(DIRECTORY - launch worlds maps rviz + config launch worlds maps rviz DESTINATION share/${PROJECT_NAME} ) diff --git a/mir_gazebo/README.md b/mir_gazebo/README.md new file mode 100644 index 00000000..9f5bb46e --- /dev/null +++ b/mir_gazebo/README.md @@ -0,0 +1,23 @@ +# mir_gazebo + +# run instructions + +The following instructions were tested under ROS2 jazzy and Gazebo harmonic. + +Run without a namespace: + +```bash +ros2 launch mir_gazebo mir_gazebo_launch.py world:=maze +``` + +Run with a namespace: + +```bash +ros2 launch mir_gazebo mir_gazebo_launch.py namespace:=robot_ns world:=maze +``` + +If no world arg is provided, the robot will spawn in an empty world. + +This will launch the simulation in Gazebo harmonic, rviz2 and a separate terminal (xterm) +to teleoperate the base with the keyboard. The Gazebo - ROS2 bridge is also launched and selected topics +are exposed to ROS2. diff --git a/mir_gazebo/config/ros_gz_bridge_config.yaml b/mir_gazebo/config/ros_gz_bridge_config.yaml new file mode 100644 index 00000000..b6c19cf4 --- /dev/null +++ b/mir_gazebo/config/ros_gz_bridge_config.yaml @@ -0,0 +1,33 @@ +- topic_name: /clock + ros_type_name: rosgraph_msgs/msg/Clock + gz_type_name: gz.msgs.Clock + direction: GZ_TO_ROS +- topic_name: /tf + ros_type_name: tf2_msgs/msg/TFMessage + gz_type_name: gz.msgs.Pose_V + direction: GZ_TO_ROS +- topic_name: cmd_vel + ros_type_name: geometry_msgs/msg/Twist + gz_type_name: gz.msgs.Twist + direction: ROS_TO_GZ +- topic_name: odom + ros_type_name: nav_msgs/msg/Odometry + gz_type_name: gz.msgs.Odometry + direction: GZ_TO_ROS +- gz_topic_name: joint_states + gz_type_name: gz.msgs.Model + ros_topic_name: mir_joint_states + ros_type_name: sensor_msgs/msg/JointState + direction: GZ_TO_ROS +- topic_name: f_scan + ros_type_name: sensor_msgs/msg/LaserScan + gz_type_name: gz.msgs.LaserScan + direction: GZ_TO_ROS +- topic_name: b_scan + ros_type_name: sensor_msgs/msg/LaserScan + gz_type_name: gz.msgs.LaserScan + direction: GZ_TO_ROS +- topic_name: imu_data + ros_type_name: sensor_msgs/msg/Imu + gz_type_name: gz.msgs.IMU + direction: GZ_TO_ROS diff --git a/mir_gazebo/launch/include/mir_gazebo_common.py b/mir_gazebo/launch/include/mir_gazebo_common.py deleted file mode 100644 index b94c3d99..00000000 --- a/mir_gazebo/launch/include/mir_gazebo_common.py +++ /dev/null @@ -1,67 +0,0 @@ -# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# * Neither the name of the the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: relffok - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - - namespace = LaunchConfiguration('namespace', default='') - - return LaunchDescription( - [ - DeclareLaunchArgument( - 'use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true' - ), - Node( - package='ira_laser_tools', - name='mir_laser_scan_merger', - executable='laserscan_multi_merger', - parameters=[ - { - 'laserscan_topics': "b_scan f_scan", - 'destination_frame': "virtual_laser_link", - 'scan_destination_topic': "scan", - 'cloud_destination_topic': "scan_cloud", - 'min_height': -0.25, - 'max_completion_time': 0.05, - 'max_merge_time_diff': 0.005, - 'use_sim_time': LaunchConfiguration('use_sim_time'), - 'best_effort': False, - } - ], - namespace=namespace, # adds namespace to topic names and frames - output='screen', - ), - ] - ) diff --git a/mir_gazebo/launch/includes/mir_robot_scan_merger_launch.py b/mir_gazebo/launch/includes/mir_robot_scan_merger_launch.py new file mode 100644 index 00000000..06122036 --- /dev/null +++ b/mir_gazebo/launch/includes/mir_robot_scan_merger_launch.py @@ -0,0 +1,72 @@ +""" +mir robot scan merger launch file based on ira_laser_tools: + subscribe to multiple LaserScan topics and merge them into a single one. + +i.e.: subscribe to /b_scan and /f_scan topics, merge them into a single /scan topic or with namespace: + subscribe to /robot_ns/b_scan and /robot_ns/f_scan topics, merge them into a single /robot_ns/scan topic + +Make sure to install via source the right version of ira_laser_tools: + github.com/relffok/ira_laser_tools.git ros2-devel branch + tested against commit: a6bfd0e1114746b70e7314366e808e709b61bb93 + +NOTE: there is a workaround in this launch file. laserscan_multi_merger (external) node ignores + the node namespace when it parses the laserscan_topics and destination_frame strings, + so a namespaced launch causes topics like /robot_ns/robot_ns/b_scan and TF lookup errors. + This launch-file workaround keeps the node in the global namespace and + pre-pends the desired robot prefix (robot_ns/…) directly to each topic/frame parameter, + ensuring the node subscribes and publishes exactly once under /robot_ns/* without code changes to ira_laser_tools. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def make_laser_merger(context): + # Resolve launch args to plain strings + ns = LaunchConfiguration('namespace').perform(context) # '' or 'robot_ns' + use_sim_time = LaunchConfiguration('use_sim_time') + + # Helper: add namespace only when the user supplied one (no leading '/') + def ns_join(name: str) -> str: + return f'{ns}/{name}' if ns else name + + # Parameters that must already contain the prefix + laserscan_topics = f'{ns_join("b_scan")} {ns_join("f_scan")}' + destination_frame = ns_join('virtual_laser_link') + scan_dest_topic = ns_join('scan') + cloud_dest_topic = ns_join('scan_cloud') + + return [Node( + package='ira_laser_tools', + executable='laserscan_multi_merger', + name='mir_laser_scan_merger', # node lives in the root namespace + # namespace is intentionally left empty -> prevents “ns/ns/topic” doubling + parameters=[{ + 'laserscan_topics': laserscan_topics, + 'destination_frame': destination_frame, + 'scan_destination_topic': scan_dest_topic, + 'cloud_destination_topic': cloud_dest_topic, + 'min_height': -0.25, + 'max_completion_time': 0.05, + 'max_merge_time_diff': 0.005, + 'use_sim_time': use_sim_time, + 'best_effort': False, + }], + output='screen', + )] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', default_value='true', + description='Use simulation (Gazebo) clock if true', + ), + DeclareLaunchArgument( + 'namespace', default_value='', + description='Optional namespace for all LaserScan I/O', + ), + OpaqueFunction(function=make_laser_merger), + ]) diff --git a/mir_gazebo/launch/includes/spawn_maze.launch.xml b/mir_gazebo/launch/includes/spawn_maze.launch.xml deleted file mode 100644 index 604f501c..00000000 --- a/mir_gazebo/launch/includes/spawn_maze.launch.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/mir_gazebo/launch/mir_gazebo_launch.py b/mir_gazebo/launch/mir_gazebo_launch.py index 20e302f6..388eed18 100644 --- a/mir_gazebo/launch/mir_gazebo_launch.py +++ b/mir_gazebo/launch/mir_gazebo_launch.py @@ -26,7 +26,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # -# Author: relffok +# Author: relffok, oscar-lima import os @@ -35,15 +35,62 @@ from launch.conditions import IfCondition from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, SetLaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource, FrontendLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution, PythonExpression from launch_ros.actions import Node +def _create_jsp_and_relay_nodes(context, *_): + ns = context.launch_configurations.get('namespace', '') + prefix = f'/{ns}' if ns else '' + + source_list = [ + f'{prefix}/mir_joint_states', + ] + + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + namespace=ns, + output='screen', + parameters=[{ + 'use_sim_time': context.launch_configurations.get('use_sim_time', 'true') == 'true', + 'source_list': source_list, + 'rate': 60.0, + }] + ) + + relay_joint_states_node = Node( + package='topic_tools', + executable='relay', + name='joint_states_relay', # no more “todo” + namespace=ns, + arguments=['joint_states', 'dynamic_joint_states'], + output='screen', + ) + + return [joint_state_publisher_node, relay_joint_states_node] + +def _create_rviz_node(context, *_): + ns = context.launch_configurations.get('namespace', '') + fixed_frame = f'{ns}/odom' if ns else 'odom' + + rviz_cfg = context.launch_configurations.get('rviz_config_file') + use_sim = context.launch_configurations.get('use_sim_time', 'true') == 'true' + + rviz_node = Node( + package='rviz2', + executable='rviz2', + namespace=ns, # RViz node itself can sit in the ns + output={'both': 'log'}, + arguments=['-d', rviz_cfg, '-f', fixed_frame], + parameters=[{'use_sim_time': use_sim}], + ) + return [rviz_node] def generate_launch_description(): mir_description_dir = get_package_share_directory('mir_description') mir_gazebo_dir = get_package_share_directory('mir_gazebo') - gazebo_ros_dir = get_package_share_directory('gazebo_ros') rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -53,6 +100,10 @@ def generate_launch_description(): 'namespace', default_value='', description='Namespace to push all topics into.' ) + declare_mir_type_arg = DeclareLaunchArgument( + 'mir_type', default_value='mir_100', description='Either mir_100 or mir_250 are supported.' + ) + declare_robot_x_arg = DeclareLaunchArgument( 'robot_x', default_value='0.0', description='Spawning position of robot (x)' ) @@ -87,27 +138,34 @@ def generate_launch_description(): declare_rviz_config_arg = DeclareLaunchArgument( 'rviz_config_file', - default_value=os.path.join(mir_description_dir, 'rviz', 'mir_visu_full.rviz'), + default_value=os.path.join(mir_gazebo_dir, 'rviz', 'mir_visualization.rviz'), description='Define rviz config file to be used.', ) declare_gui_arg = DeclareLaunchArgument('gui', default_value='true', description='Set to "false" to run headless.') + world_file = PathJoinSubstitution([ + mir_gazebo_dir, + 'worlds', + PythonExpression(['\'', LaunchConfiguration('world'), TextSubstitution(text='.world'), '\'']) + ]) + launch_gazebo_world = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(gazebo_ros_dir, 'launch', 'gazebo.launch.py')), - launch_arguments={ - 'verbose': LaunchConfiguration('verbose'), - 'gui': LaunchConfiguration('gui'), - 'world': [mir_gazebo_dir, '/worlds/', LaunchConfiguration('world'), '.world'], - }.items(), + PythonLaunchDescriptionSource( + [os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')] + ), + launch_arguments={'gz_args': ['-r -v4 ', world_file], 'on_exit_shutdown': 'true'}.items(), ) launch_mir_description = IncludeLaunchDescription( - FrontendLaunchDescriptionSource(os.path.join(mir_description_dir, 'launch', 'robot_state_publisher.launch')) + FrontendLaunchDescriptionSource(os.path.join(mir_description_dir, 'launch', 'robot_state_publisher.launch')), + launch_arguments={'mir_type':LaunchConfiguration('mir_type'), 'tf_prefix':LaunchConfiguration('namespace')}.items(), ) - launch_mir_gazebo_common = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(mir_gazebo_dir, 'launch', 'include', 'mir_gazebo_common.py')) + launch_mir_robot_scan_merger = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(mir_gazebo_dir, 'launch', 'includes', 'mir_robot_scan_merger_launch.py')), + launch_arguments={'namespace': LaunchConfiguration('namespace'), + 'use_sim_time': 'true'}.items(), ) def process_namespace(context): @@ -120,26 +178,35 @@ def process_namespace(context): return [SetLaunchConfiguration('robot_name', robot_name)] spawn_robot = Node( - package='gazebo_ros', - executable='spawn_entity.py', + package='ros_gz_sim', + executable='create', arguments=[ - '-entity', - LaunchConfiguration('robot_name'), - '-topic', - 'robot_description', - '-b', - ], # bond node to gazebo model, + '-name', LaunchConfiguration('robot_name'), + '-topic', 'robot_description', + '-x', '0.0', '-y', '0.0', '-z', '0.0', + '-R', '0.0', '-P', '0.0', '-Y', '0.0', + ], namespace=LaunchConfiguration('namespace'), output='screen', ) - launch_rviz = Node( + gz_bridge_node = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='mir_gz_bridge', + namespace=LaunchConfiguration('namespace'), + output='screen', + parameters=[{ + 'config_file': PathJoinSubstitution([ + mir_gazebo_dir, 'config', 'ros_gz_bridge_config.yaml' + ]), + 'expand_gz_topic_names': True + }] + ) + + launch_rviz = OpaqueFunction( + function=_create_rviz_node, condition=IfCondition(LaunchConfiguration('rviz_enabled')), - package='rviz2', - executable='rviz2', - output={'both': 'log'}, - arguments=['-d', rviz_config_file], - parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], ) launch_teleop = Node( @@ -153,6 +220,7 @@ def process_namespace(context): ld.add_action(OpaqueFunction(function=process_namespace)) ld.add_action(declare_namespace_arg) + ld.add_action(declare_mir_type_arg) ld.add_action(declare_robot_x_arg) ld.add_action(declare_robot_y_arg) ld.add_action(declare_robot_yaw_arg) @@ -166,8 +234,10 @@ def process_namespace(context): ld.add_action(launch_gazebo_world) ld.add_action(launch_mir_description) - ld.add_action(launch_mir_gazebo_common) + ld.add_action(OpaqueFunction(function=_create_jsp_and_relay_nodes)) + ld.add_action(launch_mir_robot_scan_merger) ld.add_action(spawn_robot) + ld.add_action(gz_bridge_node) ld.add_action(launch_rviz) ld.add_action(launch_teleop) diff --git a/mir_gazebo/launch/mir_maze_world.launch b/mir_gazebo/launch/mir_maze_world.launch deleted file mode 100644 index 1f3f2a09..00000000 --- a/mir_gazebo/launch/mir_maze_world.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/mir_gazebo/launch/ros_gz_bridge.launch b/mir_gazebo/launch/ros_gz_bridge.launch new file mode 100644 index 00000000..94368caa --- /dev/null +++ b/mir_gazebo/launch/ros_gz_bridge.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mir_gazebo/package.xml b/mir_gazebo/package.xml index 33bf5f0a..68850871 100644 --- a/mir_gazebo/package.xml +++ b/mir_gazebo/package.xml @@ -15,8 +15,11 @@ ament_cmake + ament_index_python gazebo gazebo_ros + launch + launch_ros rviz2 ira_laser_tools teleop_twist_keyboard @@ -26,6 +29,15 @@ gazebo_ros_pkgs robot_localization + controller_manager + fake_localization + joint_state_publisher + mir_driver + robot_state_publisher + ros_gz_sim + ros_gz_bridge + rqt_robot_steering + topic_tools ament_lint_auto ament_cmake_copyright ament_cmake_lint_cmake diff --git a/mir_gazebo/rviz/mir_visualization.rviz b/mir_gazebo/rviz/mir_visualization.rviz index 4db4158b..c0111e42 100644 --- a/mir_gazebo/rviz/mir_visualization.rviz +++ b/mir_gazebo/rviz/mir_visualization.rviz @@ -5,12 +5,9 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /Status1 - - /RobotModel1 - /RobotModel1/Description Topic1 - - /LaserScan1 - Splitter Ratio: 0.5 - Tree Height: 784 + Splitter Ratio: 0.536821722984314 + Tree Height: 865 - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 @@ -43,7 +40,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 + - Alpha: 0.4000000059604645 Class: rviz_default_plugins/RobotModel Collision Enabled: false Description File: "" @@ -53,7 +50,7 @@ Visualization Manager: Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: robot_description Enabled: true Links: All Links Enabled: true @@ -150,6 +147,13 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + virtual_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel TF Prefix: "" Update Interval: 0 @@ -164,32 +168,103 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/LaserScan - Color: 239; 41; 41 + Color: 165; 29; 45 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 0 Min Color: 0; 0; 0 Min Intensity: 0 - Name: LaserScan + Name: front_laser + Position Transformer: XYZ + Selectable: true + Size (Pixels): 5 + Size (m): 0.029999999329447746 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: f_scan + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 165; 29; 45 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: rear_laser Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.03999999910593033 - Style: Flat Squares + Size (m): 0.029999999329447746 + Style: Spheres Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /scan + Value: b_scan + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 165; 29; 45 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: merged_laser + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.029999999329447746 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: scan Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz_default_plugins/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: true @@ -199,22 +274,6 @@ Visualization Manager: Value: true base_link: Value: true - bl_caster_rotation_link: - Value: true - bl_caster_wheel_link: - Value: true - br_caster_rotation_link: - Value: true - br_caster_wheel_link: - Value: true - fl_caster_rotation_link: - Value: true - fl_caster_wheel_link: - Value: true - fr_caster_rotation_link: - Value: true - fr_caster_wheel_link: - Value: true front_laser_link: Value: true imu_frame: @@ -233,45 +292,36 @@ Visualization Manager: Value: true us_2_frame: Value: true - Marker Scale: 1 + virtual_laser_link: + Value: true + Marker Scale: 0.5 Name: TF Show Arrows: true Show Axes: true - Show Names: false + Show Names: true Tree: - base_footprint: - base_link: - back_laser_link: - {} - bl_caster_rotation_link: - bl_caster_wheel_link: + odom: + base_footprint: + base_link: + back_laser_link: {} - br_caster_rotation_link: - br_caster_wheel_link: + front_laser_link: {} - fl_caster_rotation_link: - fl_caster_wheel_link: + imu_link: + imu_frame: + {} + left_wheel_link: {} - fr_caster_rotation_link: - fr_caster_wheel_link: + right_wheel_link: {} - front_laser_link: - {} - imu_link: - imu_frame: + surface: + {} + us_1_frame: + {} + us_2_frame: + {} + virtual_laser_link: {} - left_wheel_link: - {} - right_wheel_link: - {} - surface: - {} - us_1_frame: - {} - us_2_frame: - {} - odom: - {} Update Interval: 0 Value: true Enabled: true @@ -289,6 +339,9 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile @@ -317,37 +370,37 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 33.73630905151367 + Distance: 5.996953964233398 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -2.321113348007202 - Y: -0.5046675205230713 - Z: 0.6754636764526367 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5397963523864746 + Pitch: 0.470398485660553 Target Frame: Value: Orbit (rviz) - Yaw: 0.004999637603759766 + Yaw: 0.9003980755805969 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 1104 Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004b30000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000206000003f2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df000001850000022efb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000003f2000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f000003f2000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000467000003f200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Tool Properties: collapsed: false Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 + collapsed: true + Width: 1651 + X: 683 + Y: 230 diff --git a/mir_gazebo/worlds/empty.world b/mir_gazebo/worlds/empty.world index 53216724..24e4618d 100644 --- a/mir_gazebo/worlds/empty.world +++ b/mir_gazebo/worlds/empty.world @@ -1,13 +1,37 @@ - - + + + + + + 0.001 + 1.0 + + + + + + + + + + - model://sun + https://fuel.gazebosim.org/1.0/OpenRobotics/models/sun - model://ground_plane + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane diff --git a/mir_gazebo/worlds/include/maze/model.sdf b/mir_gazebo/worlds/include/maze/model.sdf index b31cc0b1..8d0f5c40 100644 --- a/mir_gazebo/worlds/include/maze/model.sdf +++ b/mir_gazebo/worlds/include/maze/model.sdf @@ -18,12 +18,10 @@ 20 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 0.030536 9.925 0 0 -0 0 @@ -44,12 +42,10 @@ 20 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 9.95554 0 0 0 0 -1.5708 @@ -70,12 +66,10 @@ 20 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 0.030536 -9.925 0 0 -0 3.14159 @@ -96,12 +90,10 @@ 1.5 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 5.35089 3.21906 0 0 -0 3.14159 @@ -122,12 +114,10 @@ 5.25 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 4.67589 5.76906 0 0 -0 1.5708 @@ -148,12 +138,10 @@ 5.5 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 7.10914 4.73454 0 0 0 -1.5708 @@ -174,12 +162,10 @@ 3 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 8.53414 2.05954 0 0 -0 0 @@ -200,12 +186,10 @@ 20 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 -9.89446 0 0 0 -0 1.5708 @@ -226,12 +210,10 @@ 5.5 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 -4.35914 -2.82889 0 0 0 -1.5708 @@ -252,12 +234,10 @@ 5.75 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 -7.15914 -5.50389 0 0 -0 3.14159 @@ -278,12 +258,10 @@ 16 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 -1.89911 1.86906 0 0 -0 0 @@ -304,12 +282,10 @@ 1.5 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 6.02589 2.54406 0 0 -0 1.5708 @@ -330,12 +306,10 @@ 0.15 0.15 2.5 - - + 1 1 1 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 6.02589 3.21906 0 0 -0 0 diff --git a/mir_gazebo/worlds/maze.world b/mir_gazebo/worlds/maze.world index 9109690e..aed6eeea 100644 --- a/mir_gazebo/worlds/maze.world +++ b/mir_gazebo/worlds/maze.world @@ -1,15 +1,20 @@ + + - model://sun + https://fuel.gazebosim.org/1.0/OpenRobotics/models/sun - model://ground_plane + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane + - model://maze + model://maze + 0 0 0 0 0 0 + diff --git a/mir_navigation/config/mir_nav_params.yaml b/mir_navigation/config/mir_nav_params.yaml index b6874580..9af8d902 100644 --- a/mir_navigation/config/mir_nav_params.yaml +++ b/mir_navigation/config/mir_nav_params.yaml @@ -26,7 +26,7 @@ amcl: recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 - robot_model_type: "differential" + robot_model_type: "nav2_amcl::DifferentialMotionModel" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true @@ -65,44 +65,18 @@ bt_navigator: enable_groot_monitoring: True groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - # - # Note: Not quite correct, params that are not mentioned can not be substituted! - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_back_up_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + # These names must match the blackboard keys in your BT XML + error_code_names: + - compute_path_error_code + - follow_path_error_code + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + error_code_names: + - compute_path_error_code + - follow_path_error_code bt_navigator_rclcpp_node: @@ -297,7 +271,7 @@ planner_server: use_sim_time: True planner_plugins: ["GridBased"] GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: true allow_unknown: true @@ -313,11 +287,11 @@ recoveries_server: cycle_frequency: 10.0 recovery_plugins: ["spin", "backup", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors::BackUp" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors::Wait" global_frame: odom robot_base_frame: base_link transform_timeout: 0.1 diff --git a/mir_navigation/config/mir_nav_params_namespaced.yaml b/mir_navigation/config/mir_nav_params_namespaced.yaml index 0721ef69..9fc7016a 100644 --- a/mir_navigation/config/mir_nav_params_namespaced.yaml +++ b/mir_navigation/config/mir_nav_params_namespaced.yaml @@ -26,7 +26,7 @@ amcl: recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 - robot_model_type: "differential" + robot_model_type: "nav2_amcl::DifferentialMotionModel" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true @@ -297,7 +297,7 @@ planner_server: use_sim_time: True planner_plugins: ["GridBased"] GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: true allow_unknown: true @@ -313,11 +313,11 @@ recoveries_server: cycle_frequency: 10.0 recovery_plugins: ["spin", "backup", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors::BackUp" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors::Wait" global_frame: namespace/odom robot_base_frame: namespace/base_link transform_timeout: 0.1 diff --git a/mir_navigation/launch/include/navigation.py b/mir_navigation/launch/include/navigation.py index de97b45f..13935d7d 100644 --- a/mir_navigation/launch/include/navigation.py +++ b/mir_navigation/launch/include/navigation.py @@ -114,8 +114,8 @@ def add_prefix_to_cmd_vel(context): remappings=remappings, ), Node( - package='nav2_recoveries', - executable='recoveries_server', + package='nav2_behaviors', + executable='behavior_server', name='recoveries_server', output='screen', parameters=[configured_params],