diff --git a/nav2_minimal_tb4_description/CMakeLists.txt b/nav2_minimal_tb4_description/CMakeLists.txt index 6c52e29..daa14a9 100644 --- a/nav2_minimal_tb4_description/CMakeLists.txt +++ b/nav2_minimal_tb4_description/CMakeLists.txt @@ -6,6 +6,8 @@ find_package(ament_cmake REQUIRED) find_package(urdf REQUIRED) find_package(xacro REQUIRED) +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/tb4_description.dsv.in") + install( DIRECTORY launch meshes urdf rviz DESTINATION share/${PROJECT_NAME} diff --git a/nav2_minimal_tb4_description/hooks/tb4_description.dsv.in b/nav2_minimal_tb4_description/hooks/tb4_description.dsv.in new file mode 100644 index 0000000..3284a1c --- /dev/null +++ b/nav2_minimal_tb4_description/hooks/tb4_description.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/ diff --git a/nav2_minimal_tb4_sim/configs/tb4_bridge.yaml b/nav2_minimal_tb4_sim/configs/tb4_bridge.yaml index 10732ea..7adefb3 100644 --- a/nav2_minimal_tb4_sim/configs/tb4_bridge.yaml +++ b/nav2_minimal_tb4_sim/configs/tb4_bridge.yaml @@ -1,5 +1,5 @@ # replace clock_bridge -- ros_topic_name: "clock" +- ros_topic_name: "/clock" gz_topic_name: "/clock" ros_type_name: "rosgraph_msgs/msg/Clock" gz_type_name: "gz.msgs.Clock" diff --git a/nav2_minimal_tb4_sim/launch/spawn_tb4.launch.py b/nav2_minimal_tb4_sim/launch/spawn_tb4.launch.py index 0619253..3325bc9 100644 --- a/nav2_minimal_tb4_sim/launch/spawn_tb4.launch.py +++ b/nav2_minimal_tb4_sim/launch/spawn_tb4.launch.py @@ -14,12 +14,10 @@ # limitations under the License. import os -from pathlib import Path 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 @@ -29,7 +27,6 @@ def generate_launch_description(): sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') - desc_dir = get_package_share_directory('nav2_minimal_tb4_description') use_sim_time = LaunchConfiguration('use_sim_time') namespace = LaunchConfiguration('namespace') @@ -65,15 +62,11 @@ def generate_launch_description(): default_value='turtlebot4', description='name of the robot') - # declare_robot_sdf_cmd = DeclareLaunchArgument( - # 'robot_sdf', - # default_value=os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro'), - # description='Full path to robot sdf file to spawn the robot in gazebo') - bridge = Node( package='ros_gz_bridge', executable='parameter_bridge', name='bridge_ros_gz', + namespace=namespace, parameters=[ { 'config_file': os.path.join( @@ -89,6 +82,7 @@ def generate_launch_description(): package='ros_gz_image', executable='image_bridge', name='bridge_gz_ros_camera_image', + namespace=namespace, output='screen', parameters=[{ 'use_sim_time': use_sim_time, @@ -99,6 +93,7 @@ def generate_launch_description(): package='ros_gz_image', executable='image_bridge', name='bridge_gz_ros_camera_depth', + namespace=namespace, output='screen', parameters=[{ 'use_sim_time': use_sim_time, @@ -109,31 +104,23 @@ def generate_launch_description(): condition=IfCondition(use_simulator), package='ros_gz_sim', executable='create', + namespace=namespace, output='screen', arguments=[ - '-entity', robot_name, + '-name', robot_name, '-topic', 'robot_description', - # '-file', Command(['xacro', ' ', robot_sdf]), # TODO SDF file is unhappy, not sure why - '-robot_namespace', namespace, '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']], parameters=[{'use_sim_time': use_sim_time}] ) - set_env_vars_resources = AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', - str(Path(os.path.join(desc_dir)).parent.resolve())) - # 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(declare_use_sim_time_cmd) - ld.add_action(set_env_vars_resources) - ld.add_action(bridge) ld.add_action(camera_bridge_image) ld.add_action(camera_bridge_depth)