Following

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v39\
+              - "<< parameters.key >>-v40\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v39\
+              - "<< parameters.key >>-v40\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -532,7 +532,7 @@ jobs:
 
 _parameters:
   release_parameters: &release_parameters
-    packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|nav2_behaviors|nav2_bringup|navigation2)'"
+    packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|opennav_following|nav2_behaviors|nav2_bringup|navigation2)'"
 
 workflows:
   version: 2

nav2_behavior_tree/CMakeLists.txt

@@ -261,6 +261,12 @@ list(APPEND plugin_libs nav2_compute_route_bt_node)
 add_library(nav2_toggle_collision_monitor_service_bt_node SHARED plugins/action/toggle_collision_monitor_service.cpp)
 list(APPEND plugin_libs nav2_toggle_collision_monitor_service_bt_node)
 
+add_library(opennav_follow_action_bt_node SHARED plugins/action/follow_object_action.cpp)
+list(APPEND plugin_libs opennav_follow_action_bt_node)
+
+add_library(opennav_follow_cancel_bt_node SHARED plugins/action/follow_object_cancel_node.cpp)
+list(APPEND plugin_libs opennav_follow_cancel_bt_node)
+
 foreach(bt_plugin ${plugin_libs})
   target_include_directories(${bt_plugin}
     PRIVATE

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -67,6 +67,7 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
     "compute_path",
     "dock_robot",
     "drive_on_heading",
+    "follow_object",
     "follow_path",
     "nav_thru_poses",
     "nav_to_pose",

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_action.hpp

@@ -0,0 +1,98 @@
+// Copyright (c) 2024 Open Navigation LLC
+// Copyright (c) 2024 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_ACTION_HPP_
+
+#include <string>
+#include <memory>
+#include <vector>
+
+#include "nav2_behavior_tree/bt_action_node.hpp"
+#include "nav2_msgs/action/follow_object.hpp"
+
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief nav2_behavior_tree::BtActionNode class that wraps nav2_msgs/FollowObject
+ */
+class FollowObjectAction
+  : public nav2_behavior_tree::BtActionNode<nav2_msgs::action::FollowObject>
+{
+  using Action = nav2_msgs::action::FollowObject;
+  using ActionResult = Action::Result;
+
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::FollowObjectAction
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param action_name Action name this node creates a client for
+   * @param conf BT node configuration
+   */
+  FollowObjectAction(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @brief Function to perform some user-defined operation on tick
+   */
+  void on_tick() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon successful completion of the action
+   */
+  BT::NodeStatus on_success() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+        BT::InputPort<std::string>(
+          "pose_topic", "dynamic_pose", "Topic to publish the pose of the object to follow"),
+        BT::InputPort<std::string>(
+          "tracked_frame", "Target frame to follow (Optional, used if pose_topic is not set)"),
+        BT::InputPort<float>(
+          "max_duration", 0.0, "The maximum duration to follow the object (Optional)"),
+
+        BT::OutputPort<ActionResult::_total_elapsed_time_type>(
+          "total_elapsed_time", "Total elapsed time"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "error_code_id", "Error code"),
+        BT::OutputPort<std::string>(
+          "error_msg", "Error message"),
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_cancel_node.hpp

@@ -0,0 +1,61 @@
+// Copyright (c) 2024 Open Navigation LLC
+// Copyright (c) 2024 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_CANCEL_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_CANCEL_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_msgs/action/follow_object.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowObject
+ */
+class FollowObjectCancel
+  : public nav2_behavior_tree::BtCancelActionNode<nav2_msgs::action::FollowObject>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::FollowObject
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param action_name Action name this node creates a client for
+   * @param conf BT node configuration
+   */
+  FollowObjectCancel(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_OBJECT_CANCEL_NODE_HPP_

nav2_behavior_tree/plugins/action/follow_object_action.cpp

@@ -0,0 +1,82 @@
+// Copyright (c) 2024 Open Navigation LLC
+// Copyright (c) 2024 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/action/follow_object_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+FollowObjectAction::FollowObjectAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<Action>(xml_tag_name, action_name, conf)
+{
+}
+
+void FollowObjectAction::on_tick()
+{
+  // Get core inputs about what to perform
+  getInput("pose_topic", goal_.pose_topic);
+  getInput("tracked_frame", goal_.tracked_frame);
+  double max_duration;
+  getInput("max_duration", max_duration);
+
+  // Populate the input message
+  goal_.max_duration = rclcpp::Duration::from_seconds(max_duration);
+}
+
+BT::NodeStatus FollowObjectAction::on_success()
+{
+  setOutput("total_elapsed_time", result_.result->total_elapsed_time);
+  setOutput("error_code_id", ActionResult::NONE);
+  setOutput("error_msg", "");
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus FollowObjectAction::on_aborted()
+{
+  setOutput("total_elapsed_time", result_.result->total_elapsed_time);
+  setOutput("error_code_id", result_.result->error_code);
+  setOutput("error_msg", result_.result->error_msg);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus FollowObjectAction::on_cancelled()
+{
+  setOutput("total_elapsed_time", result_.result->total_elapsed_time);
+  setOutput("error_code_id", ActionResult::NONE);
+  setOutput("error_msg", "");
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<nav2_behavior_tree::FollowObjectAction>(
+        name, "follow_object", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::FollowObjectAction>(
+    "FollowObject", builder);
+}

nav2_behavior_tree/plugins/action/follow_object_cancel_node.cpp

@@ -0,0 +1,47 @@
+// Copyright (c) 2024 Open Navigation LLC
+// Copyright (c) 2024 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <memory>
+
+#include "nav2_behavior_tree/plugins/action/follow_object_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+FollowObjectCancel::FollowObjectCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: nav2_behavior_tree::BtCancelActionNode<nav2_msgs::action::FollowObject>(
+    xml_tag_name, action_name, conf)
+{
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<nav2_behavior_tree::FollowObjectCancel>(
+        name, "follow_object", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::FollowObjectCancel>(
+    "CancelFollowObject", builder);
+}

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -94,3 +94,7 @@ plugin_add_test(test_progress_checker_selector_node test_progress_checker_select
 plugin_add_test(test_toggle_collision_monitor_service
   test_toggle_collision_monitor_service.cpp
   nav2_toggle_collision_monitor_service_bt_node)
+
+plugin_add_test(test_follow_object_action test_follow_object_action.cpp opennav_follow_action_bt_node)
+
+plugin_add_test(test_follow_object_cancel_node test_follow_object_cancel_node.cpp opennav_follow_cancel_bt_node)