diff --git a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst index a9b325a18..a8a9870a2 100644 --- a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst +++ b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst @@ -75,11 +75,12 @@ Parameters ============== ============================= Type Default -------------- ----------------------------- - string "cmd_vel_raw" + string "cmd_vel_smoothed" ============== ============================= Description: Input ``cmd_vel`` topic with desired robot velocity. + Please note, pre-``Jazzy`` this was set to ``cmd_vel_raw`` by default. :cmd_vel_out_topic: @@ -403,7 +404,7 @@ Here is an example of configuration YAML for the Collision Monitor. ros__parameters: base_frame_id: "base_footprint" odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_raw" + cmd_vel_in_topic: "cmd_vel_smoothed" cmd_vel_out_topic: "cmd_vel" state_topic: "collision_monitor_state" transform_tolerance: 0.5 diff --git a/tutorials/docs/using_collision_monitor.rst b/tutorials/docs/using_collision_monitor.rst index cff777573..24516a1cb 100644 --- a/tutorials/docs/using_collision_monitor.rst +++ b/tutorials/docs/using_collision_monitor.rst @@ -81,7 +81,7 @@ The whole ``nav2_collision_monitor/params/collision_monitor_params.yaml`` file i use_sim_time: True base_frame_id: "base_footprint" odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_raw" + cmd_vel_in_topic: "cmd_vel_smoothed" cmd_vel_out_topic: "cmd_vel" transform_tolerance: 0.5 source_timeout: 5.0 @@ -111,35 +111,44 @@ Preparing Nav2 stack ==================== The Collision Monitor is designed to operate below Nav2 as an independent safety node. -This acts as a filter on the ``cmd_vel`` topic coming out of the Controller Server. -If no such zone is triggered, then the Controller's ``cmd_vel`` is used. -Else, it is scaled or set to stop as appropriate. -For correct operation of the Collision Monitor with the Controller, it is required to add the ``cmd_vel -> cmd_vel_raw`` remapping to the ``navigation_launch.py`` bringup script as presented below: +It acts as a filter for the ``cmd_vel`` messages from the controller to avoid potential collisions. +If no such zone is triggered, then the ``cmd_vel`` message is used. +Else, it is scaled or set to stop as appropriate. + +By default, the Collision Monitor is configured for usage with the Nav2 bringup package, running in parallel with the ``navigation_launch.py`` launch file. For correct operation of the Collision Monitor with the Velocity Smoother, it is required to remove the Velocity Smoother's ``cmd_vel_smoothed`` remapping in the ``navigation_launch.py`` bringup script as presented below. This will make the output topic of the Velocity Smoother to be untouched, which will be the input to the newly added Collision Monitor: .. code-block:: python Node( - package='nav2_controller', - executable='controller_server', + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', output='screen', respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], - + remappings=remappings + [('cmd_vel', 'cmd_vel_raw')]), + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + + [('cmd_vel', 'cmd_vel_nav')]), ... ComposableNode( - package='nav2_controller', - plugin='nav2_controller::ControllerServer', - name='controller_server', + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', parameters=[configured_params], - + remappings=remappings + [('cmd_vel', 'cmd_vel_raw')]), + remappings=remappings + + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + + [('cmd_vel', 'cmd_vel_nav')]), + +If you have changed Collision Monitor's default ``cmd_vel_in_topic`` and ``cmd_vel_out_topic`` configuration, make sure Velocity Smoother's default output topic ``cmd_vel_smoothed`` should match to the input velocity ``cmd_vel_in_topic`` parameter value of the Collision Monitor node, and the output velocity ``cmd_vel_out_topic`` parameter value should be actual ``cmd_vel`` to fit the replacement. -Please note, that the remapped ``cmd_vel_raw`` topic should match to the input velocity ``cmd_vel_in_topic`` parameter value of the Collision Monitor node, and the output velocity ``cmd_vel_out_topic`` parameter value should be actual ``cmd_vel`` to fit the replacement. +.. note:: As the Collision Monitor acts as a safety node, it must be the last link in the velocity message post-processing chain, making it the node that publishes to the ``cmd_vel`` topic. It could be placed after smoothed velocity, as in our demonstration, or after non-smoothed velocity from Controller Server, e.g. if Velocity Smoother was not enabled in the system, or going after any other module in custom configuration producing the end-velocity. Therefore, in any custom Nav2 launch configuration, the last node publishing to the ``cmd_vel`` topic, should be remapped to publish to the Collision Monitor input topic configured by ``cmd_vel_in_topic`` ROS-parameter (``cmd_vel_smoothed`` by default). Demo Execution ============== -Once Collision Monitor node has been tuned and ``cmd_vel`` topics remapped, Collision Monitor node is ready to run. +Once Collision Monitor node has been tuned and ``cmd_vel`` topics adjusted, Collision Monitor node is ready to run. For that, run Nav2 stack as written in :ref:`getting_started`: .. code-block:: bash