diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 8728581df12..425dac4edee 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -18,12 +18,10 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler -from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit from launch.events import Shutdown from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from nav2_common.launch import ReplaceString def generate_launch_description(): @@ -32,7 +30,6 @@ def generate_launch_description(): # Create the launch configuration variables namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') rviz_config_file = LaunchConfiguration('rviz_config') use_sim_time = LaunchConfiguration('use_sim_time') @@ -46,12 +43,6 @@ def generate_launch_description(): ), ) - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack', - ) - declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), @@ -65,68 +56,37 @@ def generate_launch_description(): # Launch rviz start_rviz_cmd = Node( - condition=UnlessCondition(use_namespace), package='rviz2', executable='rviz2', + namespace=namespace, arguments=['-d', rviz_config_file], output='screen', parameters=[{'use_sim_time': use_sim_time}], - ) - - namespaced_rviz_config_file = ReplaceString( - source_file=rviz_config_file, - replacements={'': ('/', namespace)}, - ) - - start_namespaced_rviz_cmd = Node( - condition=IfCondition(use_namespace), - package='rviz2', - executable='rviz2', - namespace=namespace, - arguments=['-d', namespaced_rviz_config_file], - parameters=[{'use_sim_time': use_sim_time}], - output='screen', remappings=[ - ('/map', 'map'), ('/tf', 'tf'), ('/tf_static', 'tf_static'), - ('/goal_pose', 'goal_pose'), - ('/clicked_point', 'clicked_point'), - ('/initialpose', 'initialpose'), ], ) exit_event_handler = RegisterEventHandler( - condition=UnlessCondition(use_namespace), event_handler=OnProcessExit( target_action=start_rviz_cmd, on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), ), ) - exit_event_handler_namespaced = RegisterEventHandler( - condition=IfCondition(use_namespace), - event_handler=OnProcessExit( - target_action=start_namespaced_rviz_cmd, - on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), - ), - ) - # 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_rviz_config_file_cmd) ld.add_action(declare_use_sim_time_cmd) # Add any conditioned actions ld.add_action(start_rviz_cmd) - ld.add_action(start_namespaced_rviz_cmd) # Add other nodes and processes we need ld.add_action(exit_event_handler) - ld.add_action(exit_event_handler_namespaced) return ld diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 2027f4c0e1c..b0e2fbf2f6f 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -42,7 +42,7 @@ bt_navigator: ros__parameters: global_frame: map robot_base_frame: base_link - odom_topic: /odom + odom_topic: odom bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 @@ -211,7 +211,7 @@ local_costmap: mark_threshold: 0 observation_sources: scan scan: - topic: /scan + topic: scan max_obstacle_height: 2.0 clearing: True marking: True @@ -241,7 +241,7 @@ global_costmap: enabled: True observation_sources: scan scan: - topic: /scan + topic: scan max_obstacle_height: 2.0 clearing: True marking: True @@ -362,7 +362,7 @@ collision_monitor: FootprintApproach: type: "polygon" action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" + footprint_topic: "local_costmap/published_footprint" time_before_collision: 1.2 simulation_time_step: 0.1 min_points: 6 diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index c8dd3768583..df1bc58d5dc 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -58,7 +58,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: robot_description Enabled: false Links: All Links Enabled: true @@ -148,7 +148,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /scan + Value: scan Use Fixed Frame: true Use rainbow: true Value: true @@ -181,7 +181,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Best Effort - Value: /mobile_base/sensors/bumper_pointcloud + Value: mobile_base/sensors/bumper_pointcloud Use Fixed Frame: true Use rainbow: true Value: true @@ -199,13 +199,13 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /map + Value: map Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /map_updates + Value: map_updates Use Timestamp: false Value: true - Alpha: 1 @@ -222,7 +222,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort - Value: /particle_cloud + Value: particle_cloud Value: true - Class: rviz_common/Group Displays: @@ -240,13 +240,13 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /global_costmap/costmap + Value: global_costmap/costmap Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /global_costmap/costmap_updates + Value: global_costmap/costmap_updates Use Timestamp: false Value: true - Alpha: 0.30000001192092896 @@ -263,13 +263,13 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /downsampled_costmap + Value: downsampled_costmap Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /downsampled_costmap_updates + Value: downsampled_costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -298,7 +298,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /plan + Value: plan Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -329,7 +329,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /global_costmap/voxel_marked_cloud + Value: global_costmap/voxel_marked_cloud Use Fixed Frame: true Use rainbow: true Value: true @@ -344,7 +344,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /global_costmap/published_footprint + Value: global_costmap/published_footprint Value: false Enabled: true Name: Global Planner @@ -364,13 +364,13 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /local_costmap/costmap + Value: local_costmap/costmap Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /local_costmap/costmap_updates + Value: local_costmap/costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -399,7 +399,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /local_plan + Value: local_plan Value: true - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -411,7 +411,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /marker + Value: marker Value: false - Alpha: 1 Class: rviz_default_plugins/Polygon @@ -424,7 +424,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /local_costmap/published_footprint + Value: local_costmap/published_footprint Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -455,7 +455,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /local_costmap/voxel_marked_cloud + Value: local_costmap/voxel_marked_cloud Use Fixed Frame: true Use rainbow: true Value: true @@ -475,7 +475,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /intel_realsense_r200_depth/image_raw + Value: intel_realsense_r200_depth/image_raw Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -506,7 +506,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /intel_realsense_r200_depth/points + Value: intel_realsense_r200_depth/points Use Fixed Frame: true Use rainbow: true Value: true @@ -522,7 +522,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /waypoints + Value: waypoints Value: true Enabled: true Global Options: @@ -545,7 +545,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /initialpose + Value: initialpose - Class: rviz_default_plugins/PublishPoint Single click: true Topic: @@ -553,7 +553,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /clicked_point + Value: clicked_point - Class: nav2_rviz_plugins/GoalTool Transformation: Current: diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 6321305dd79..eb78fbcc510 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -189,6 +189,17 @@ void ObstacleLayer::onInitialize() node->get_parameter(name_ + "." + source + "." + "raytrace_min_range", raytrace_min_range); node->get_parameter(name_ + "." + source + "." + "raytrace_max_range", raytrace_max_range); + // If the user passed a relative topic, use it as relative to this node's parent namespace. + // For example: + // * User chosen namespace is `tb4`. + // * User chosen topic is `scan`. + // * Node namespace will be `/tb4/global_costmap`. + // * Topic will be remapped to `/tb4/scan` rather than `/tb4/global_costmap/scan` + if (topic[0] != '/') { + std::string node_namespace = node->get_namespace(); + std::string parent_namespace = node_namespace.substr(0, node_namespace.rfind("/")); + topic = parent_namespace + "/" + topic; + } RCLCPP_DEBUG( logger_,