From e913dc4d811f8ae54c77e09db5951b976e1d2785 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Tue, 30 Sep 2025 13:34:41 +0200 Subject: [PATCH 1/6] Added following server doc Signed-off-by: Alberto Tudela --- configuration/index.rst | 1 + .../packages/configuring-following-server.rst | 239 ++++++++++++++++++ migration/Kilted.rst | 10 + .../navigation2_dynamic_point_following.rst | 102 +++++++- 4 files changed, 346 insertions(+), 6 deletions(-) create mode 100644 configuration/packages/configuring-following-server.rst diff --git a/configuration/index.rst b/configuration/index.rst index 544e9cd15d..a16c91aa41 100644 --- a/configuration/index.rst +++ b/configuration/index.rst @@ -67,4 +67,5 @@ Others packages/configuring-amcl.rst packages/configuring-coverage-server.rst + packages/configuring-following-server.rst packages/configuring-loopback-sim.rst diff --git a/configuration/packages/configuring-following-server.rst b/configuration/packages/configuring-following-server.rst new file mode 100644 index 0000000000..4116f572f8 --- /dev/null +++ b/configuration/packages/configuring-following-server.rst @@ -0,0 +1,239 @@ +.. _configuring_following_server: + +Following Server +################ + +Source code on Github_. + +.. _Github: https://github.com/open-navigation/opennav_following + +The Following Server in ``opennav_following`` implements a server for following dynamic objects or specific reference frames. +This server allows the robot to follow and maintain a determined distance from a detected object or specific frame, +using topic-based detection techniques or coordinate frame tracking. +The server is designed to be called by a BT application or autonomy application to follow moving objects. + +Parameters +********** + +:controller_frequency: + + ============== ============== + Type Default + -------------- -------------- + double 50.0 + ============== ============== + + Description + Control frequency (Hz) for the following control loop. + +:detection_timeout: + + ============== ============== + Type Default + -------------- -------------- + double 2.0 + ============== ============== + + Description + Timeout (s) to wait for detection of the object to follow. + +:rotate_to_object_timeout: + + ============== ============== + Type Default + -------------- -------------- + double 10.0 + ============== ============== + + Description + Timeout (s) to rotate searching for the object when detection is lost. + +:static_object_timeout: + + ============== ============== + Type Default + -------------- -------------- + double -1.0 + ============== ============== + + Description + Timeout (s) to stop following when the object remains static. If -1.0, the robot will follow indefinitely. + +:linear_tolerance: + + ============== ============== + Type Default + -------------- -------------- + double 0.15 + ============== ============== + + Description + Linear tolerance (m) to consider that the target position has been reached. + +:angular_tolerance: + + ============== ============== + Type Default + -------------- -------------- + double 0.15 + ============== ============== + + Description + Angular tolerance (rad) to consider that the target orientation has been reached. + +:max_retries: + + ============== ============== + Type Default + -------------- -------------- + int 3 + ============== ============== + + Description + Maximum number of retries when detection or control fails. + +:base_frame: + + ============== ============== + Type Default + -------------- -------------- + string "base_link" + ============== ============== + + Description + Robot's base frame for control law. + +:fixed_frame: + + ============== ============== + Type Default + -------------- -------------- + string "odom" + ============== ============== + + Description + Fixed frame to use, recommended to be a smooth odometry frame **not** map. + +:filter_coef: + + ============== ============== + Type Default + -------------- -------------- + double 0.1 + ============== ============== + + Description + Filter coefficient for smoothing object pose detections. + +:desired_distance: + + ============== ============== + Type Default + -------------- -------------- + double 1.0 + ============== ============== + + Description + Desired distance (m) to maintain from the followed object. + +:skip_orientation: + + ============== ============== + Type Default + -------------- -------------- + bool true + ============== ============== + + Description + If true, ignore the detected object's orientation and point toward it from the robot's position. + +:search_by_rotating: + + ============== ============== + Type Default + -------------- -------------- + bool false + ============== ============== + + Description + If true, the robot will rotate in place when it loses object detection to try to find it again. + +:search_angle: + + ============== ============== + Type Default + -------------- -------------- + double M_PI_2 + ============== ============== + + Description + Maximum angle (rad) to rotate when searching for the object. + +:odom_topic: + + ============== ============== + Type Default + -------------- -------------- + string "odom" + ============== ============== + + Description + Odometry topic to use for obtaining the robot's current velocity. + +:odom_duration: + + ============== =========================== + Type Default + -------------- --------------------------- + double 0.3 + ============== =========================== + + Description + Time (s) to buffer odometry commands to estimate the robot speed. + +:transform_tolerance: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description + Time with which to post-date the transform that is published, to indicate that this transform is valid into the future. + +Example +******* +.. code-block:: yaml + + following_server: + ros__parameters: + controller_frequency: 50.0 + detection_timeout: 2.0 + rotate_to_object_timeout: 10.0 + static_object_timeout: 30.0 # -1.0 for indefinite following + linear_tolerance: 0.15 + angular_tolerance: 0.15 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + filter_coef: 0.1 + desired_distance: 1.0 + skip_orientation: true + search_by_rotating: false + odom_topic: "odom" + odom_duration: 0.3 + transform_tolerance: 0.1 + + # Controller configuration (inherited from docking controller) + controller: + k_phi: 3.0 + k_delta: 2.0 + beta: 0.4 + lambda: 2.0 + v_linear_min: 0.1 + v_linear_max: 0.5 + v_angular_max: 1.0 + slowdown_radius: 0.15 + use_collision_detection: false + transform_tolerance: 0.1 diff --git a/migration/Kilted.rst b/migration/Kilted.rst index 8157918a07..e76a3897a8 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -421,3 +421,13 @@ An example usage in a Behavior Tree XML file: .. code-block:: xml + + +Following Server +---------------- + +`PR #5565 `_ adds a new Following Server into the ``opennav_following`` package. +The Following Server implements a server for following dynamic objects or specific reference frames. +This server allows the robot to follow and maintain a determined distance from a detected object or specific frame, using topic-based detection techniques or coordinate frame tracking. + +The information about Following Server parameters set-up could be found at :ref:`configuring_following_server` configuration guide. diff --git a/tutorials/docs/navigation2_dynamic_point_following.rst b/tutorials/docs/navigation2_dynamic_point_following.rst index a1a41cbef5..4c00cda37e 100644 --- a/tutorials/docs/navigation2_dynamic_point_following.rst +++ b/tutorials/docs/navigation2_dynamic_point_following.rst @@ -3,8 +3,98 @@ Dynamic Object Following ************************ -- `Overview`_ -- `Tutorial Steps`_ +Using Following server +====================== + +- `Following Server Overview`_ +- `Following Server Tutorial Steps`_ + +Following Server Overview +========================= + +The Following server is a specialized server in Nav2 designed to handle dynamic object following tasks. It dynamically follows an object while maintaining a defined distance specified by the ``desired_distance`` parameter. The server abstracts away the complexities of tracking and following moving objects, allowing users to focus on higher-level navigation goals. + +The ``FollowObject`` action can be called using two different input methods: + +- **pose_topic**: Topic to publish the pose of the object to follow. When using this method, the server subscribes to the specified topic of type ``geometry_msgs::msg::PoseStamped`` containing the pose of the object to follow. +- **tracked_frame**: Target frame to follow. When using this method, the server directly follows the specified frame name using the transform tree. + +The Following server uses the same controller as the Docking server, leveraging the ``SmoothControlLaw`` to generate velocity commands for smoothly following the target object. This ensures consistent and predictable motion behavior across Nav2's specialized servers. + +**Recovery Mechanism**: If the object becomes undetectable for a period of time (e.g., when the pose topic stops publishing), the server implements a recovery mechanism where the robot will rotate left and right in a search pattern until it relocates the target object. + + +Following Server Tutorial Steps +=============================== + +To use the Following server, you need to configure it in your behavior tree. This involves setting up the necessary action nodes and parameters to ensure smooth following behavior. The ``FollowObject`` action node provides two different approaches for object tracking. + +Method 1: Using pose_topic +-------------------------- + +The simplest behavior tree for following a dynamic object using a pose topic is as follows: + +.. code-block:: xml + + + + + + + +In this configuration: + +- ``pose_topic``: Specifies the topic (``/person_pose``) where ``geometry_msgs::msg::PoseStamped`` messages containing the object's pose are published +- ``max_duration``: Duration to run the action (0.0 means indefinitely) + +Method 2: Using tracked_frame +----------------------------- + +Alternatively, you can follow a specific TF frame directly: + +.. code-block:: xml + + + + + + + +In this configuration: + +- ``tracked_frame``: Specifies the frame name (``person_frame``) to follow using the transform tree +- ``max_duration``: Duration to run the action (0.0 means indefinitely) + +**Additional Parameters** + +The ``FollowObject`` action supports additional parameters for fine-tuning: + +- ``desired_distance``: Distance to maintain from the target object (in meters) +- ``detection_timeout``: Time to wait without detecting the object before triggering recovery behavior +- ``rotate_to_object_timeout``: Maximum time allowed for the rotation recovery mechanism to find the object +- ``static_object_timeout``: Time threshold to determine if an object has been stationary +- ``search_by_rotating``: Enable/disable the rotational search recovery mechanism when the object is lost + +Recovery behavior is automatically activated when the target becomes undetectable, utilizing the search parameters above. + +**Following Server in Action** + +The following video demonstrates the Following Server functionality in a real-world scenario: + +.. raw:: html + +

+
+ +
+

+ + +Using ComputePathToPose and FollowPath +====================================== + +- `ComputePathToPose Overview`_ +- `ComputePathToPose Tutorial Steps`_ .. raw:: html @@ -14,8 +104,8 @@ Dynamic Object Following -Overview -======== +ComputePathToPose Overview +========================== This tutorial shows how to use Nav2 for a different task other than going from point A to point B. In this case, we will use Nav2 to follow a moving object at a distance indefinitely. @@ -45,8 +135,8 @@ send the initial pose to the ``NavigateToPose`` action, and update it on a topic .. image:: images/navigation2_dynamic_point_following/main_diagram.png :width: 48% -Tutorial Steps -============== +ComputePathToPose Tutorial Steps +================================ 0- Create the Behavior Tree --------------------------- From cbdf9cc194ff34e44bca9fbee56ab22c36852ae7 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Wed, 1 Oct 2025 12:14:23 +0200 Subject: [PATCH 2/6] Update configuration/packages/configuring-following-server.rst Co-authored-by: Steve Macenski Signed-off-by: Alberto Tudela --- configuration/packages/configuring-following-server.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configuration/packages/configuring-following-server.rst b/configuration/packages/configuring-following-server.rst index 4116f572f8..1bc7219b7e 100644 --- a/configuration/packages/configuring-following-server.rst +++ b/configuration/packages/configuring-following-server.rst @@ -7,7 +7,7 @@ Source code on Github_. .. _Github: https://github.com/open-navigation/opennav_following -The Following Server in ``opennav_following`` implements a server for following dynamic objects or specific reference frames. +The Following Server in ``opennav_following`` implements a server for following dynamic objects from a detection topic or specific reference frame. This server allows the robot to follow and maintain a determined distance from a detected object or specific frame, using topic-based detection techniques or coordinate frame tracking. The server is designed to be called by a BT application or autonomy application to follow moving objects. From 414ddda1f26ca2d8f5bb0127e9e772901c21ba67 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Wed, 1 Oct 2025 12:14:33 +0200 Subject: [PATCH 3/6] Update tutorials/docs/navigation2_dynamic_point_following.rst Co-authored-by: Steve Macenski Signed-off-by: Alberto Tudela --- tutorials/docs/navigation2_dynamic_point_following.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/docs/navigation2_dynamic_point_following.rst b/tutorials/docs/navigation2_dynamic_point_following.rst index 4c00cda37e..9cbb0dc359 100644 --- a/tutorials/docs/navigation2_dynamic_point_following.rst +++ b/tutorials/docs/navigation2_dynamic_point_following.rst @@ -17,7 +17,7 @@ The Following server is a specialized server in Nav2 designed to handle dynamic The ``FollowObject`` action can be called using two different input methods: - **pose_topic**: Topic to publish the pose of the object to follow. When using this method, the server subscribes to the specified topic of type ``geometry_msgs::msg::PoseStamped`` containing the pose of the object to follow. -- **tracked_frame**: Target frame to follow. When using this method, the server directly follows the specified frame name using the transform tree. +- **tracked_frame**: Target TF frame to follow. When using this method, the server directly follows the specified frame name using the transform tree. The Following server uses the same controller as the Docking server, leveraging the ``SmoothControlLaw`` to generate velocity commands for smoothly following the target object. This ensures consistent and predictable motion behavior across Nav2's specialized servers. From 1c8bcdf8b396eb3464133e48bb8658bab05ddcdd Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Wed, 1 Oct 2025 12:32:06 +0200 Subject: [PATCH 4/6] Update links Signed-off-by: Alberto Tudela --- .../packages/configuring-following-server.rst | 10 ++++++- .../navigation2_dynamic_point_following.rst | 26 ++++++++++++------- 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/configuration/packages/configuring-following-server.rst b/configuration/packages/configuring-following-server.rst index 1bc7219b7e..ae6d3a790a 100644 --- a/configuration/packages/configuring-following-server.rst +++ b/configuration/packages/configuring-following-server.rst @@ -5,13 +5,21 @@ Following Server Source code on Github_. -.. _Github: https://github.com/open-navigation/opennav_following +.. _Github: https://github.com/ros-navigation/navigation2/nav2_following/opennav_following The Following Server in ``opennav_following`` implements a server for following dynamic objects from a detection topic or specific reference frame. This server allows the robot to follow and maintain a determined distance from a detected object or specific frame, using topic-based detection techniques or coordinate frame tracking. The server is designed to be called by a BT application or autonomy application to follow moving objects. +.. raw:: html + +

+
+ +
+

+ Parameters ********** diff --git a/tutorials/docs/navigation2_dynamic_point_following.rst b/tutorials/docs/navigation2_dynamic_point_following.rst index 9cbb0dc359..f6530d32ce 100644 --- a/tutorials/docs/navigation2_dynamic_point_following.rst +++ b/tutorials/docs/navigation2_dynamic_point_following.rst @@ -3,14 +3,19 @@ Dynamic Object Following ************************ -Using Following server +Nav2 provides two different approaches for dynamic object following tasks: + +1. **Following Server**: A specialized server that handles dynamic object following with built-in recovery mechanisms and smooth control. +2. **ComputePathToPose and FollowPath**: A behavior tree-based approach using traditional navigation components for custom following behaviors. + +Using Following Server ====================== - `Following Server Overview`_ - `Following Server Tutorial Steps`_ Following Server Overview -========================= +------------------------- The Following server is a specialized server in Nav2 designed to handle dynamic object following tasks. It dynamically follows an object while maintaining a defined distance specified by the ``desired_distance`` parameter. The server abstracts away the complexities of tracking and following moving objects, allowing users to focus on higher-level navigation goals. @@ -25,12 +30,12 @@ The Following server uses the same controller as the Docking server, leveraging Following Server Tutorial Steps -=============================== +------------------------------- To use the Following server, you need to configure it in your behavior tree. This involves setting up the necessary action nodes and parameters to ensure smooth following behavior. The ``FollowObject`` action node provides two different approaches for object tracking. Method 1: Using pose_topic --------------------------- +`````````````````````````` The simplest behavior tree for following a dynamic object using a pose topic is as follows: @@ -48,7 +53,7 @@ In this configuration: - ``max_duration``: Duration to run the action (0.0 means indefinitely) Method 2: Using tracked_frame ------------------------------ +````````````````````````````` Alternatively, you can follow a specific TF frame directly: @@ -89,6 +94,7 @@ The following video demonstrates the Following Server functionality in a real-wo +---- Using ComputePathToPose and FollowPath ====================================== @@ -105,7 +111,7 @@ Using ComputePathToPose and FollowPath ComputePathToPose Overview -========================== +-------------------------- This tutorial shows how to use Nav2 for a different task other than going from point A to point B. In this case, we will use Nav2 to follow a moving object at a distance indefinitely. @@ -136,10 +142,10 @@ send the initial pose to the ``NavigateToPose`` action, and update it on a topic :width: 48% ComputePathToPose Tutorial Steps -================================ +-------------------------------- 0- Create the Behavior Tree ---------------------------- +```````````````````````````` Let's start from this simple behavior tree. This behavior tree replans a new path at 1 hz and passes that path to the controller to follow: @@ -219,7 +225,7 @@ Now, you may save this behavior tree and use it in our navigation task. For reference, this exact behavior tree is `made available `_ to you batteries included in the ``nav2_bt_navigator`` package. 1- Setup Rviz clicked point ---------------------------- +``````````````````````````` We are going to use RViz instead of a full application so you can test at home without finding a detector to get started. We will use the "clicked point" button on the toolbar to substitute object detections to provide goal updates to Nav2. This button allows you to publish coordinates in the topic ``/clicked_point``. This point needs to be sent to the behavior tree, using the program ``clicked_point_to_pose``, from `this repo `_. Clone @@ -230,7 +236,7 @@ this repo in your workspace, build, and type in a terminal. Optionally, you can remap this topic in your rviz configuration file to ``goal_updates``. 2- Run Dynamic Object Following in Nav2 Simulation --------------------------------------------------- +`````````````````````````````````````````````````` Start Nav2 in one terminal: From 9b819ec577eff9aab582653197d709e3f4b064b4 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Thu, 2 Oct 2025 09:34:49 +0200 Subject: [PATCH 5/6] Update migration guide Signed-off-by: Alberto Tudela --- migration/Kilted.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/migration/Kilted.rst b/migration/Kilted.rst index e76a3897a8..777b13e0c2 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -431,3 +431,5 @@ The Following Server implements a server for following dynamic objects or specif This server allows the robot to follow and maintain a determined distance from a detected object or specific frame, using topic-based detection techniques or coordinate frame tracking. The information about Following Server parameters set-up could be found at :ref:`configuring_following_server` configuration guide. + +The tutorial for the Following Server has been recently updated. For the latest instructions and examples, see :ref:`navigation2-dynamic-point-following`. From 6e6c51bd4917a522e36efacc58247ef3629ca342 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Mon, 6 Oct 2025 09:31:03 +0200 Subject: [PATCH 6/6] Added functions to the Simple Commander API Signed-off-by: Alberto Tudela --- commander_api/index.rst | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/commander_api/index.rst b/commander_api/index.rst index 5e568fc47b..ea7353914f 100644 --- a/commander_api/index.rst +++ b/commander_api/index.rst @@ -147,6 +147,12 @@ New as of September 2023: the simple navigator constructor will accept a `namesp +---------------------------------------+----------------------------------------------------------------------------+ | toggleCollisionMonitor(enable) | Toggles the collision monitor on (`True`) or off (`False`). | +---------------------------------------+----------------------------------------------------------------------------+ +| followObjectByTopic(topic, | Requests the robot to follow an object by subscribing to a given topic. | +| max_duration=0) | | ++---------------------------------------+----------------------------------------------------------------------------+ +| followObjectByFrame(frame, | Requests the robot to follow an object by a given frame. | +| max_duration=0) | | ++---------------------------------------+----------------------------------------------------------------------------+ | waitUntilNav2Active( | Blocks until Nav2 is completely online and lifecycle nodes are in the | | navigator='bt_navigator', | active state. To be used in conjunction with autostart or external | | localizer='amcl') | lifecycle bringup. Custom navigator and localizer nodes can be specified |