Skip to content

Commit

Permalink
adding get pose from path (#574)
Browse files Browse the repository at this point in the history
  • Loading branch information
SteveMacenski authored Jul 9, 2024
1 parent df3439a commit 81f19e4
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 1 deletion.
52 changes: 52 additions & 0 deletions configuration/packages/bt-plugins/actions/GetPoseFromPath.rst
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
.. _bt_getposefrompath_action:

GetPoseFromPath
===============

Gets a pose from a particular index on the path. Use ``-1`` to get the last pose, ``-2`` for second to last, and so on.

Input Ports
***********

:path:

============= =======
Type Default
------------- -------
nav_msgs/Path N/A
============= =======

Description
Path to extract pose from

:index:

====== =======
Type Default
------ -------
int 0
====== =======

Description
Index from path to use. Use ``-1`` to get the last pose, ``-2`` for second to last, and so on.

Output Ports
------------

:pose:

========================= =======
Type Default
------------------------- -------
geometry_msgs/PoseStamped N/A
========================= =======

Description
Pose from path, with the Path's set header.

Example
-------

.. code-block:: xml
<GetPoseFromPath path="{path}" index="-1" pose="{goal}"/>
1 change: 1 addition & 0 deletions configuration/packages/configuring-bt-xml.rst
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ Action Plugins
bt-plugins/actions/CancelDriveOnHeading.rst
bt-plugins/actions/CancelAssistedTeleop.rst
bt-plugins/actions/Smooth.rst
bt-plugins/actions/GetPoseFromPath.rst

Condition Plugins
*****************
Expand Down
9 changes: 8 additions & 1 deletion migration/Jazzy.rst
Original file line number Diff line number Diff line change
Expand Up @@ -23,4 +23,11 @@ Here we can see the working demo of the plugin:

.. image:: images/docking_panel.gif

.. attention:: If the docking server is unavailable, then the combo box of the dock type will be empty.
.. attention:: If the docking server is unavailable, then the combo box of the dock type will be empty.

New BT Nodes
************

Below is a list of new BT Nodes added:

- ``GetPoseFromPath``: An action to get a particular pose from an input path.
3 changes: 3 additions & 0 deletions plugins/index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -410,6 +410,8 @@ Behavior Tree Nodes
+--------------------------------------------+---------------------+------------------------------------------+
| `Compute Complete Coverage Path Action`_ | Steve Macenski | Calls coverage planner server |
+--------------------------------------------+---------------------+------------------------------------------+
| `Get Pose From Path Action`_ | Marc Morcos | Extracts a pose from a path |
+--------------------------------------------+---------------------+------------------------------------------+

.. _Back Up Action: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/back_up_action.cpp
.. _Drive On Heading Action: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp
Expand Down Expand Up @@ -442,6 +444,7 @@ Behavior Tree Nodes
.. _Cancel Assisted Teleop Action: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp
.. _Cancel Complete Coverage Action: https://github.com/open-navigation/opennav_coverage/blob/main/opennav_coverage_bt/src/cancel_complete_coverage_path.cpp
.. _Compute Complete Coverage Path Action: https://github.com/open-navigation/opennav_coverage/blob/main/opennav_coverage_bt/src/compute_complete_coverage_path.cpp
.. _Get Pose From Path Action: https://github.com/ros-navigation/navigation2/blob/main/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp


+------------------------------------+--------------------+------------------------+
Expand Down

0 comments on commit 81f19e4

Please sign in to comment.