From 86573823b434ddfb6a37221f53beacd6ed0d1dee Mon Sep 17 00:00:00 2001 From: Marc Morcos <30278842+MarcM0@users.noreply.github.com> Date: Fri, 5 Jul 2024 13:25:29 -0400 Subject: [PATCH] Behavior tree node for extracting pose from path (#4518) * add get pose from path action Signed-off-by: MarcM0 * cleanup from PR suggestions Signed-off-by: MarcM0 * Updates for main compatibility Signed-off-by: MarcM0 * Lint and build fix Signed-off-by: MarcM0 * More Lint and warnings Signed-off-by: MarcM0 * More Lint and build Signed-off-by: MarcM0 * remove code left over from older file Signed-off-by: MarcM0 * fix test blackboard var name Signed-off-by: MarcM0 * only populate pose frame if empty Signed-off-by: MarcM0 * lint Signed-off-by: MarcM0 --------- Signed-off-by: MarcM0 (cherry picked from commit 12a9c1d805847709e3b82f8dcfbb43c67b5b2937) --- nav2_behavior_tree/CMakeLists.txt | 3 + .../action/get_pose_from_path_action.hpp | 55 ++++++ .../action/get_pose_from_path_action.cpp | 79 +++++++++ .../test/plugins/action/CMakeLists.txt | 4 + .../action/test_get_pose_from_path_action.cpp | 156 ++++++++++++++++++ 5 files changed, 297 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp create mode 100644 nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 72b5c1b6f2..2cc6f6689a 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -171,6 +171,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) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp new file mode 100644 index 0000000000..091360b479 --- /dev/null +++ b/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 +#include +#include + +#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("path", "Path to extract pose from"), + BT::OutputPort("pose", "Stamped Extracted Pose"), + BT::InputPort("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_ diff --git a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp new file mode 100644 index 0000000000..cab0311a37 --- /dev/null +++ b/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 +#include +#include + +#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(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("GetPoseFromPath"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 4aaf185306..513a19caec 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/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}) diff --git a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp new file mode 100644 index 0000000000..39f6df2a3c --- /dev/null +++ b/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 +#include +#include +#include +#include + +#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("get_pose_from_path_action_test_fixture"); + factory_ = std::make_shared(); + + 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( + "node", + node_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "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 factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr GetPoseFromPathTestFixture::node_ = nullptr; +BT::NodeConfiguration * GetPoseFromPathTestFixture::config_ = nullptr; +std::shared_ptr GetPoseFromPathTestFixture::factory_ = nullptr; +std::shared_ptr GetPoseFromPathTestFixture::tree_ = nullptr; + +TEST_F(GetPoseFromPathTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new path and set it on blackboard + nav_msgs::msg::Path path; + std::vector 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("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("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; +}