Behavior tree node for extracting pose from path (#4518) (#4524)

* add get pose from path action

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* cleanup from PR suggestions

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* Updates for main compatibility

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* Lint and build fix

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* More Lint and warnings

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* More Lint and build

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* remove code left over from older file

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* fix test blackboard var name

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* only populate pose frame if empty

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

* lint

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

---------

Signed-off-by: MarcM0 <marc.morcos9@gmail.com>
(cherry picked from commit 12a9c1d)

Co-authored-by: Marc Morcos <30278842+MarcM0@users.noreply.github.com>

nav2_behavior_tree/CMakeLists.txt

@@ -183,6 +183,9 @@ list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node)
 add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp)
 list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node)
 
+add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp)
+list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node)
+
 add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
 list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp

@@ -0,0 +1,55 @@
+// Copyright (c) 2024 Marc Morcos
+//
+// 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__GET_POSE_FROM_PATH_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_
+
+#include <vector>
+#include <memory>
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "behaviortree_cpp/action_node.h"
+#include "nav_msgs/msg/path.h"
+
+namespace nav2_behavior_tree
+{
+
+class GetPoseFromPath : public BT::ActionNodeBase
+{
+public:
+  GetPoseFromPath(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<nav_msgs::msg::Path>("path", "Path to extract pose from"),
+      BT::OutputPort<geometry_msgs::msg::PoseStamped>("pose", "Stamped Extracted Pose"),
+      BT::InputPort<int>("index", 0, "Index of pose to extract from. -1 is end of list"),
+    };
+  }
+
+private:
+  void halt() override {}
+  BT::NodeStatus tick() override;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_

nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp

@@ -0,0 +1,79 @@
+// Copyright (c) 2024 Marc Morcos
+//
+// 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 <limits>
+
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GetPoseFromPath::GetPoseFromPath(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(name, conf)
+{
+}
+
+inline BT::NodeStatus GetPoseFromPath::tick()
+{
+  setStatus(BT::NodeStatus::RUNNING);
+
+  nav_msgs::msg::Path input_path;
+  getInput("path", input_path);
+
+  int pose_index;
+  getInput("index", pose_index);
+
+  if (input_path.poses.empty()) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Account for negative indices
+  if(pose_index < 0) {
+    pose_index = input_path.poses.size() + pose_index;
+  }
+
+  // out of bounds index
+  if(pose_index < 0 || static_cast<unsigned>(pose_index) >= input_path.poses.size()) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // extract pose
+  geometry_msgs::msg::PoseStamped output_pose;
+  output_pose = input_path.poses[pose_index];
+
+  // populate pose frame from path if necessary
+  if(output_pose.header.frame_id.empty()) {
+    output_pose.header.frame_id = input_path.header.frame_id;
+  }
+
+
+  setOutput("pose", output_pose);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GetPoseFromPath>("GetPoseFromPath");
+}

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -92,6 +92,10 @@ ament_add_gtest(test_remove_passed_goals_action test_remove_passed_goals_action.
 target_link_libraries(test_remove_passed_goals_action nav2_remove_passed_goals_action_bt_node)
 ament_target_dependencies(test_remove_passed_goals_action ${dependencies})
 
+ament_add_gtest(test_get_pose_from_path_action test_get_pose_from_path_action.cpp)
+target_link_libraries(test_get_pose_from_path_action nav2_get_pose_from_path_action_bt_node)
+ament_target_dependencies(test_get_pose_from_path_action ${dependencies})
+
 ament_add_gtest(test_planner_selector_node test_planner_selector_node.cpp)
 target_link_libraries(test_planner_selector_node nav2_planner_selector_bt_node)
 ament_target_dependencies(test_planner_selector_node ${dependencies})

nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp

@@ -0,0 +1,156 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2021 Samsung Research America
+// Copyright (c) 2024 Marc Morcos
+//
+// 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 <gtest/gtest.h>
+#include <memory>
+#include <set>
+#include <string>
+#include <vector>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "behaviortree_cpp/bt_factory.h"
+
+#include "utils/test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp"
+#include "utils/test_behavior_tree_fixture.hpp"
+
+class GetPoseFromPathTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("get_pose_from_path_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    config_ = new BT::NodeConfiguration();
+
+    // Create the blackboard that will be shared by all of the nodes in the tree
+    config_->blackboard = BT::Blackboard::create();
+    // Put items on the blackboard
+    config_->blackboard->set<rclcpp::Node::SharedPtr>(
+      "node",
+      node_);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::GetPoseFromPath>(
+          name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::GetPoseFromPath>(
+      "GetPoseFromPath", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr GetPoseFromPathTestFixture::node_ = nullptr;
+BT::NodeConfiguration * GetPoseFromPathTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> GetPoseFromPathTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> GetPoseFromPathTestFixture::tree_ = nullptr;
+
+TEST_F(GetPoseFromPathTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <GetPoseFromPath path="{path}" pose="{pose}" index="{index}" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new path and set it on blackboard
+  nav_msgs::msg::Path path;
+  std::vector<geometry_msgs::msg::PoseStamped> goals;
+  goals.resize(2);
+  goals[0].pose.position.x = 1.0;
+  goals[1].pose.position.x = 2.0;
+  path.poses = goals;
+  path.header.frame_id = "test_frame_1";
+  config_->blackboard->set("path", path);
+
+  config_->blackboard->set("index", 0);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // the goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // check if returned pose is correct
+  geometry_msgs::msg::PoseStamped pose;
+  EXPECT_TRUE(config_->blackboard->get<geometry_msgs::msg::PoseStamped>("pose", pose));
+  EXPECT_EQ(pose.header.frame_id, "test_frame_1");
+  EXPECT_EQ(pose.pose.position.x, 1.0);
+
+  // halt node so another goal can be sent
+  tree_->haltTree();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+
+  // try last element
+  config_->blackboard->set("index", -1);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // check if returned pose is correct
+  EXPECT_TRUE(config_->blackboard->get<geometry_msgs::msg::PoseStamped>("pose", pose));
+  EXPECT_EQ(pose.header.frame_id, "test_frame_1");
+  EXPECT_EQ(pose.pose.position.x, 2.0);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}