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 | 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..ae6d3a790a --- /dev/null +++ b/configuration/packages/configuring-following-server.rst @@ -0,0 +1,247 @@ +.. _configuring_following_server: + +Following Server +################ + +Source code on Github_. + +.. _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 +********** + +: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..777b13e0c2 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -421,3 +421,15 @@ 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. + +The tutorial for the Following Server has been recently updated. For the latest instructions and examples, see :ref:`navigation2-dynamic-point-following`. diff --git a/tutorials/docs/navigation2_dynamic_point_following.rst b/tutorials/docs/navigation2_dynamic_point_following.rst index a1a41cbef5..f6530d32ce 100644 --- a/tutorials/docs/navigation2_dynamic_point_following.rst +++ b/tutorials/docs/navigation2_dynamic_point_following.rst @@ -3,8 +3,104 @@ Dynamic Object Following ************************ -- `Overview`_ -- `Tutorial Steps`_ +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. + +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 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. + +**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 +110,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,11 +141,11 @@ 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 ---------------------------- +```````````````````````````` 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: @@ -129,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 @@ -140,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: