diff --git a/nav2_bringup/launch/tb3_gz_robot_launch.py b/nav2_bringup/launch/tb3_gz_robot_launch.py new file mode 100644 index 0000000000..dbc4f19ddc --- /dev/null +++ b/nav2_bringup/launch/tb3_gz_robot_launch.py @@ -0,0 +1,106 @@ +# Copyright (C) 2023 Open Source Robotics Foundation +# +# 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 os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import AppendEnvironmentVariable +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + + +def generate_launch_description(): + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + use_simulator = LaunchConfiguration('use_simulator') + robot_name = LaunchConfiguration('robot_name') + robot_sdf = LaunchConfiguration('robot_sdf') + pose = {'x': LaunchConfiguration('x_pose', default='-2.00'), + 'y': LaunchConfiguration('y_pose', default='-0.50'), + 'z': LaunchConfiguration('z_pose', default='0.01'), + 'R': LaunchConfiguration('roll', default='0.00'), + 'P': LaunchConfiguration('pitch', default='0.00'), + 'Y': LaunchConfiguration('yaw', default='0.00')} + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_simulator_cmd = DeclareLaunchArgument( + 'use_simulator', + default_value='True', + description='Whether to start the simulator') + + declare_robot_name_cmd = DeclareLaunchArgument( + 'robot_name', + default_value='turtlebot3_waffle', + description='name of the robot') + + declare_robot_sdf_cmd = DeclareLaunchArgument( + 'robot_sdf', + default_value=os.path.join(bringup_dir, 'worlds', 'gz_waffle.sdf'), + description='Full path to robot sdf file to spawn the robot in gazebo') + + pkg_nav2_bringup = get_package_share_directory('nav2_bringup') + bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + parameters=[ + { + 'config_file': os.path.join( + pkg_nav2_bringup, 'params', 'turtlebot3_waffle_bridge.yaml' + ), + } + ], + output='screen', + ) + + spawn_model = Node( + condition=IfCondition(use_simulator), + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', robot_name, + '-file', robot_sdf, + '-robot_namespace', namespace, + '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], + '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']] + ) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(get_package_share_directory('turtlebot3_gazebo'), + 'models')) + + # Create the launch description and populate + ld = LaunchDescription() + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_robot_name_cmd) + ld.add_action(declare_robot_sdf_cmd) + ld.add_action(declare_use_simulator_cmd) + + ld.add_action(set_env_vars_resources) + + ld.add_action(bridge) + ld.add_action(spawn_model) + return ld diff --git a/nav2_bringup/launch/tb3_gz_simulation_launch.py b/nav2_bringup/launch/tb3_gz_simulation_launch.py new file mode 100644 index 0000000000..a6147518d9 --- /dev/null +++ b/nav2_bringup/launch/tb3_gz_simulation_launch.py @@ -0,0 +1,306 @@ +# Copyright (C) 2023 Open Source Robotics Foundation +# +# 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. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os +import tempfile + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + AppendEnvironmentVariable, + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression + +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + launch_dir = os.path.join(bringup_dir, 'launch') + # This checks that tb3 exists needed for the URDF. If not using TB3, its safe to remove. + _ = get_package_share_directory('turtlebot3_gazebo') + + # Create the launch configuration variables + slam = LaunchConfiguration('slam') + namespace = LaunchConfiguration('namespace') + use_namespace = LaunchConfiguration('use_namespace') + map_yaml_file = LaunchConfiguration('map') + use_sim_time = LaunchConfiguration('use_sim_time') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration('rviz_config_file') + use_simulator = LaunchConfiguration('use_simulator') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + world = LaunchConfiguration('world') + pose = { + 'x': LaunchConfiguration('x_pose', default='-2.00'), + 'y': LaunchConfiguration('y_pose', default='-0.50'), + 'z': LaunchConfiguration('z_pose', default='0.01'), + 'R': LaunchConfiguration('roll', default='0.00'), + 'P': LaunchConfiguration('pitch', default='0.00'), + 'Y': LaunchConfiguration('yaw', default='0.00'), + } + robot_name = LaunchConfiguration('robot_name') + robot_sdf = LaunchConfiguration('robot_sdf') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + 'use_namespace', + default_value='false', + description='Whether to apply a namespace to the navigation stack', + ) + + declare_slam_cmd = DeclareLaunchArgument( + 'slam', default_value='False', description='Whether run a SLAM' + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + description='Full path to map file to load', + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation (Gazebo) clock if true', + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='True', + description='Whether to use composed bringup', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + description='Full path to the RVIZ config file to use', + ) + + declare_use_simulator_cmd = DeclareLaunchArgument( + 'use_simulator', + default_value='True', + description='Whether to start the simulator', + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + 'use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher', + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', default_value='True', description='Whether to execute gzclient)' + ) + + declare_world_cmd = DeclareLaunchArgument( + 'world', + # TODO(orduno) Switch back once ROS argument passing has been fixed upstream + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91 + # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), + # worlds/turtlebot3_worlds/waffle.model') + default_value=os.path.join(bringup_dir, 'worlds', 'gz_world_only.sdf.xacro'), + description='Full path to world model file to load', + ) + + declare_robot_name_cmd = DeclareLaunchArgument( + 'robot_name', default_value='turtlebot3_waffle', description='name of the robot' + ) + + declare_robot_sdf_cmd = DeclareLaunchArgument( + 'robot_sdf', + default_value=os.path.join(bringup_dir, 'worlds', 'gz_waffle.sdf'), + description='Full path to robot sdf file to spawn the robot in gazebo', + ) + + urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + namespace=namespace, + output='screen', + parameters=[ + {'use_sim_time': use_sim_time, 'robot_description': robot_description} + ], + remappings=remappings, + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'rviz_config': rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'slam': slam, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + }.items(), + ) + # The SDF file for the world is a xacro file because we wanted to + # conditionally load the SceneBroadcaster plugin based on wheter we're + # running in headless mode. But currently, the Gazebo command line doesn't + # take SDF strings for worlds, so the output of xacro needs to be saved into + # a temporary file and passed to Gazebo. + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + gazebo_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items(), + condition=IfCondition(use_simulator)) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(get_package_share_directory('turtlebot3_gazebo'), + 'models')) + gazebo_client = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + [use_simulator, ' and not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), + ) + + gz_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'tb3_gz_robot_launch.py')), + launch_arguments={'namespace': namespace, + 'use_simulator': use_simulator, + 'use_sim_time': use_sim_time, + 'robot_name': robot_name, + 'robot_sdf': robot_sdf, + 'x_pose': pose['x'], + 'y_pose': pose['y'], + 'z_pose': pose['z'], + 'roll': pose['R'], + 'pitch': pose['P'], + 'yaw': pose['Y']}.items()) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_robot_name_cmd) + ld.add_action(declare_robot_sdf_cmd) + ld.add_action(declare_use_respawn_cmd) + + ld.add_action(set_env_vars_resources) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(gz_robot) + ld.add_action(gazebo_server) + ld.add_action(gazebo_client) + + # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 56c8602eeb..3216746350 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -15,10 +15,15 @@ navigation2 launch_ros + diff_drive_controller + joint_state_broadcaster launch_ros navigation2 nav2_common + ros_gz_bridge + ros_gz_sim slam_toolbox + xacro diff --git a/nav2_bringup/params/turtlebot3_waffle_bridge.yaml b/nav2_bringup/params/turtlebot3_waffle_bridge.yaml new file mode 100644 index 0000000000..843699f8eb --- /dev/null +++ b/nav2_bringup/params/turtlebot3_waffle_bridge.yaml @@ -0,0 +1,51 @@ +# replace clock_bridge +- ros_topic_name: "clock" + gz_topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS + +# no equivalent in TB3 - add +- ros_topic_name: "joint_states" + gz_topic_name: "joint_states" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS + +# replace odom_bridge - check gz topic name +# gz topic published by DiffDrive plugin +- ros_topic_name: "odom" + gz_topic_name: "/odom" + ros_type_name: "nav_msgs/msg/Odometry" + gz_type_name: "gz.msgs.Odometry" + direction: GZ_TO_ROS + +# replace odom_tf_bridge - check gz and ros topic names +# gz topic published by DiffDrive plugin +# prefix ros2 topic with gz +- ros_topic_name: "tf" + gz_topic_name: "/tf" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: GZ_TO_ROS + +# replace imu_bridge - check gz topic name +- ros_topic_name: "imu" + gz_topic_name: "/imu" + ros_type_name: "sensor_msgs/msg/Imu" + gz_type_name: "gz.msgs.IMU" + direction: GZ_TO_ROS + +# replace lidar_bridge +- ros_topic_name: "scan" + gz_topic_name: "/scan" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "gz.msgs.LaserScan" + direction: GZ_TO_ROS + +# replace cmd_vel_bridge +- ros_topic_name: "cmd_vel" + gz_topic_name: "/cmd_vel" + ros_type_name: "geometry_msgs/msg/Twist" + gz_type_name: "gz.msgs.Twist" + direction: ROS_TO_GZ diff --git a/nav2_bringup/worlds/gz_waffle.sdf b/nav2_bringup/worlds/gz_waffle.sdf new file mode 100644 index 0000000000..283dc4e4a7 --- /dev/null +++ b/nav2_bringup/worlds/gz_waffle.sdf @@ -0,0 +1,490 @@ + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + + + + -0.064 0 0.048 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 1.0 + + + + -0.064 0 0.048 0 0 0 + + + 0.265 0.265 0.089 + + + + + + -0.064 0 0 0 0 0 + + + model://turtlebot3_common/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + 1 1 1 + + + + + + + true + 200 + imu + imu_link + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + + + + + -0.064 0 0.121 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + + -0.052 0 0.111 0 0 0 + + + 0.0508 + 0.055 + + + + + + -0.064 0 0.121 0 0 0 + + + model://turtlebot3_common/meshes/lds.dae + 0.001 0.001 0.001 + + + + 1 1 1 + + + + + true + true + -0.064 0 0.15 0 0 0 + 5 + scan + base_scan + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.00001 + 20.0 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + + + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_common/meshes/tire.dae + 0.001 0.001 0.001 + + + + 1 1 1 + + + + + + + + 0.0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_common/meshes/tire.dae + 0.001 0.001 0.001 + + + + 1 1 1 + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + + 0.069 -0.047 0.107 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 0 0 + + + 0.008 0.130 0.022 + + + + + 0.069 -0.047 0.107 0 0 0 + + + 1 + 5 + 0.064 -0.047 0.107 0 0 0 + camera_depth_frame + + 1.047 + + 320 + 240 + + + + + + 19.4 + + + + + 0.001 + 5.0 + + + + + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 0 0 + + 0 0 1 + + + + + + wheel_left_joint + wheel_right_joint + 0.287 + 0.033 + 1 + -1 + 2 + -2 + 0.46 + -0.46 + 1.9 + -1.9 + /cmd_vel + /odom + tf + odom + base_footprint + 30 + + + + wheel_left_joint + wheel_right_joint + joint_states + 30 + + + + diff --git a/nav2_bringup/worlds/gz_world_only.sdf.xacro b/nav2_bringup/worlds/gz_world_only.sdf.xacro new file mode 100644 index 0000000000..d93c7facf0 --- /dev/null +++ b/nav2_bringup/worlds/gz_world_only.sdf.xacro @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + ogre2 + + + + + + 0 + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + 10 + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 + + + + 0.001 + 1 + 1000.0 + + + + 1 + + model://turtlebot3_world + + + + +