From d97330e0c4f403f6a13ff934dd8e5e7bdf841c07 Mon Sep 17 00:00:00 2001 From: MarcM0 Date: Thu, 4 Jul 2024 12:18:18 -0400 Subject: [PATCH 01/10] add get pose from path action Signed-off-by: MarcM0 --- nav2_behavior_tree/CMakeLists.txt | 3 + .../action/get_pose_from_path_action.hpp | 56 +++++++ .../action/get_pose_from_path_action.cpp | 74 +++++++++ .../test/plugins/action/CMakeLists.txt | 4 + .../action/test_get_pose_from_path_action.cpp | 156 ++++++++++++++++++ 5 files changed, 293 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 91fc49b71d..3950082ab8 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/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) 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..a6bbbaff7a --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp @@ -0,0 +1,56 @@ +// 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_v3/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; + int poseIndex = 0; +}; + +} // 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..1bed82693d --- /dev/null +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -0,0 +1,74 @@ +// 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) +{ + getInput("index", poseIndex); +} + +inline BT::NodeStatus GetPoseFromPath::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + nav_msgs::msg::Path inputPath; + getInput("path", inputPath); + + if (inputPath.poses.empty()) { + return BT::NodeStatus::FAILURE; + } + + int tempPoseIndex = poseIndex; + if(tempPoseIndex<0){ //Account for negative indices + tempPoseIndex = inputPath.poses.size()+tempPoseIndex; + } + + //out of bounds index + if(tempPoseIndex<0 || (unsigned)tempPoseIndex>=inputPath.poses.size()){ + return BT::NodeStatus::FAILURE; + } + + //extract pose + geometry_msgs::msg::PoseStamped outputPose; + outputPose = inputPath.poses[tempPoseIndex]; + + //populate pose frame from path + outputPose.header.frame_id = inputPath.header.frame_id; + + setOutput("pose", outputPose); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/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..4e194f447d --- /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_v3/bt_factory.h" + +#include "utils/test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.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); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + + // check if returned pose is correct + geometry_msgs::msg::PoseStamped pose; + 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_->rootNode()->halt(); + 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 + 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; +} From d68b277627a370d0b722f9df20b2c31373e06181 Mon Sep 17 00:00:00 2001 From: MarcM0 Date: Thu, 4 Jul 2024 12:29:33 -0400 Subject: [PATCH 02/10] cleanup from PR suggestions Signed-off-by: MarcM0 --- .../action/get_pose_from_path_action.hpp | 1 - .../action/get_pose_from_path_action.cpp | 25 ++++++++++--------- 2 files changed, 13 insertions(+), 13 deletions(-) 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 index a6bbbaff7a..4715b0eb22 100644 --- 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 @@ -48,7 +48,6 @@ class GetPoseFromPath : public BT::ActionNodeBase private: void halt() override {} BT::NodeStatus tick() override; - int poseIndex = 0; }; } // namespace nav2_behavior_tree 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 index 1bed82693d..ab3156682e 100644 --- a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -29,38 +29,39 @@ GetPoseFromPath::GetPoseFromPath( const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf) { - getInput("index", poseIndex); } inline BT::NodeStatus GetPoseFromPath::tick() { setStatus(BT::NodeStatus::RUNNING); + + nav_msgs::msg::Path input_path; + getInput("path", input_path); - nav_msgs::msg::Path inputPath; - getInput("path", inputPath); + int pose_index; + getInput("index", pose_index); - if (inputPath.poses.empty()) { + if (input_path.poses.empty()) { return BT::NodeStatus::FAILURE; } - int tempPoseIndex = poseIndex; - if(tempPoseIndex<0){ //Account for negative indices - tempPoseIndex = inputPath.poses.size()+tempPoseIndex; + if(pose_index<0){ //Account for negative indices + pose_index = input_path.poses.size()+pose_index; } //out of bounds index - if(tempPoseIndex<0 || (unsigned)tempPoseIndex>=inputPath.poses.size()){ + if(pose_index<0 || static_cast(pose_index>=input_path.poses.size())){ return BT::NodeStatus::FAILURE; } //extract pose - geometry_msgs::msg::PoseStamped outputPose; - outputPose = inputPath.poses[tempPoseIndex]; + geometry_msgs::msg::PoseStamped output_pose; + output_pose = input_path.poses[pose_index]; //populate pose frame from path - outputPose.header.frame_id = inputPath.header.frame_id; + output_pose.header.frame_id = input_path.header.frame_id; - setOutput("pose", outputPose); + setOutput("pose", output_pose); return BT::NodeStatus::SUCCESS; } From 29bbf6abc6eaf92a16d9dd43a593a78bdbc4f7ae Mon Sep 17 00:00:00 2001 From: MarcM0 Date: Thu, 4 Jul 2024 13:19:41 -0400 Subject: [PATCH 03/10] Updates for main compatibility Signed-off-by: MarcM0 --- .../plugins/action/get_pose_from_path_action.hpp | 2 +- .../plugins/action/get_pose_from_path_action.cpp | 2 +- .../plugins/action/test_get_pose_from_path_action.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) 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 index 4715b0eb22..091360b479 100644 --- 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 @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "nav_msgs/msg/path.h" namespace nav2_behavior_tree 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 index ab3156682e..7fa2f26608 100644 --- a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -68,7 +68,7 @@ inline BT::NodeStatus GetPoseFromPath::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GetPoseFromPath"); 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 index 4e194f447d..a0484b9a64 100644 --- 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 @@ -23,10 +23,11 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#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 { @@ -78,8 +79,7 @@ class GetPoseFromPathTestFixture : public ::testing::Test rclcpp::Node::SharedPtr GetPoseFromPathTestFixture::node_ = nullptr; BT::NodeConfiguration * GetPoseFromPathTestFixture::config_ = nullptr; -std::shared_ptr GetPoseFromPathTestFixture::factory_ = - nullptr; +std::shared_ptr GetPoseFromPathTestFixture::factory_ = nullptr; std::shared_ptr GetPoseFromPathTestFixture::tree_ = nullptr; TEST_F(GetPoseFromPathTestFixture, test_tick) @@ -87,7 +87,7 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) // create tree std::string xml_txt = R"( - + From 1e2e8399ef1c676d589b1914c96cb12f16fdfcf5 Mon Sep 17 00:00:00 2001 From: MarcM0 Date: Thu, 4 Jul 2024 13:25:44 -0400 Subject: [PATCH 04/10] Lint and build fix Signed-off-by: MarcM0 --- .../plugins/action/get_pose_from_path_action.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) 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 index 7fa2f26608..624192955f 100644 --- a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -34,7 +34,7 @@ GetPoseFromPath::GetPoseFromPath( inline BT::NodeStatus GetPoseFromPath::tick() { setStatus(BT::NodeStatus::RUNNING); - + nav_msgs::msg::Path input_path; getInput("path", input_path); @@ -45,22 +45,23 @@ inline BT::NodeStatus GetPoseFromPath::tick() return BT::NodeStatus::FAILURE; } - if(pose_index<0){ //Account for negative indices + // 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())){ + // out of bounds index + if(pose_index < 0 || static_cast(pose_index) >= input_path.poses.size()){ return BT::NodeStatus::FAILURE; } - //extract pose + // extract pose geometry_msgs::msg::PoseStamped output_pose; output_pose = input_path.poses[pose_index]; - //populate pose frame from path + // populate pose frame from path output_pose.header.frame_id = input_path.header.frame_id; - + setOutput("pose", output_pose); return BT::NodeStatus::SUCCESS; From 6ad053c8bf1325132f4b0ce89c688eea8ff19a32 Mon Sep 17 00:00:00 2001 From: MarcM0 Date: Thu, 4 Jul 2024 13:50:34 -0400 Subject: [PATCH 05/10] More Lint and warnings Signed-off-by: MarcM0 --- .../plugins/action/get_pose_from_path_action.cpp | 2 +- .../test/plugins/action/test_get_pose_from_path_action.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) 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 index 624192955f..c9556ff8f9 100644 --- a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -46,7 +46,7 @@ inline BT::NodeStatus GetPoseFromPath::tick() } // Account for negative indices - if(pose_index < 0){ + if(pose_index < 0){ pose_index = input_path.poses.size()+pose_index; } 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 index a0484b9a64..a27a680112 100644 --- 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 @@ -101,9 +101,10 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) goals.resize(2); goals[0].pose.position.x = 1.0; goals[1].pose.position.x = 2.0; - path.poses=goals; + path.poses = goals; path.header.frame_id = "test_frame_1"; config_->blackboard->set("path", path); + config_->blackboard->set("index", 0); // tick until node succeeds @@ -117,7 +118,7 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) // check if returned pose is correct geometry_msgs::msg::PoseStamped pose; - config_->blackboard->get("pose", 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); @@ -135,7 +136,7 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); // check if returned pose is correct - config_->blackboard->get("pose", pose); + EXPECT_TRUE(config_->blackboard->get("pose", pose)); EXPECT_EQ(pose.header.frame_id, "test_frame_1"); EXPECT_EQ(pose.pose.position.x, 2.0); } From 84aae9ef8c4a60cf42488bd33401eac6c667b45d Mon Sep 17 00:00:00 2001 From: MarcM0 Date: Thu, 4 Jul 2024 13:58:04 -0400 Subject: [PATCH 06/10] More Lint and build Signed-off-by: MarcM0 --- .../plugins/action/get_pose_from_path_action.cpp | 6 +++--- .../test/plugins/action/test_get_pose_from_path_action.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) 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 index c9556ff8f9..cfc70ec18b 100644 --- a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -46,12 +46,12 @@ inline BT::NodeStatus GetPoseFromPath::tick() } // Account for negative indices - if(pose_index < 0){ - pose_index = input_path.poses.size()+pose_index; + 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()){ + if(pose_index < 0 || static_cast(pose_index) >= input_path.poses.size()) { return BT::NodeStatus::FAILURE; } 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 index a27a680112..de50dabccb 100644 --- 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 @@ -123,7 +123,7 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) EXPECT_EQ(pose.pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // try last element From feef656f346aa8ee537c804290333b97a0a5f552 Mon Sep 17 00:00:00 2001 From: MarcM0 Date: Thu, 4 Jul 2024 14:24:24 -0400 Subject: [PATCH 07/10] remove code left over from older file Signed-off-by: MarcM0 --- .../test/plugins/action/test_get_pose_from_path_action.cpp | 1 - 1 file changed, 1 deletion(-) 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 index de50dabccb..168113545f 100644 --- 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 @@ -114,7 +114,6 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); // check if returned pose is correct geometry_msgs::msg::PoseStamped pose; From d3fe9622c83bd4e4b62516fd4d6a11d818b87f63 Mon Sep 17 00:00:00 2001 From: MarcM0 Date: Thu, 4 Jul 2024 14:55:28 -0400 Subject: [PATCH 08/10] fix test blackboard var name Signed-off-by: MarcM0 --- .../test/plugins/action/test_get_pose_from_path_action.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 168113545f..39f6df2a3c 100644 --- 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 @@ -89,7 +89,7 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) R"( - + )"; From bed6eab10112196afe06828313fb203e08b7bec0 Mon Sep 17 00:00:00 2001 From: MarcM0 Date: Thu, 4 Jul 2024 15:00:22 -0400 Subject: [PATCH 09/10] only populate pose frame if empty Signed-off-by: MarcM0 --- .../plugins/action/get_pose_from_path_action.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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 index cfc70ec18b..dd85725aa5 100644 --- a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -59,8 +59,11 @@ inline BT::NodeStatus GetPoseFromPath::tick() geometry_msgs::msg::PoseStamped output_pose; output_pose = input_path.poses[pose_index]; - // populate pose frame from path - output_pose.header.frame_id = input_path.header.frame_id; + // 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); From 0b0a0f2f626dd10628b35eccd8bae7ff089a36f6 Mon Sep 17 00:00:00 2001 From: MarcM0 Date: Thu, 4 Jul 2024 15:02:20 -0400 Subject: [PATCH 10/10] lint Signed-off-by: MarcM0 --- nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index dd85725aa5..cab0311a37 100644 --- a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -63,7 +63,7 @@ inline BT::NodeStatus GetPoseFromPath::tick() if(output_pose.header.frame_id.empty()) { output_pose.header.frame_id = input_path.header.frame_id; } - + setOutput("pose", output_pose);