Skip to content

Commit 2c13b51

Browse files
mergify[bot]Marc-Morcos
mergify[bot]
and
Marc-Morcos
authored
Behavior tree node for extracting pose from path (#4518) (#4525)
* 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>
1 parent 48a31f9 commit 2c13b51

File tree

5 files changed

+297
-0
lines changed

5 files changed

+297
-0
lines changed

‎nav2_behavior_tree/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -171,6 +171,9 @@ list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node)
171171
add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp)
172172
list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node)
173173

174+
add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp)
175+
list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node)
176+
174177
add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
175178
list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)
176179

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

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
// Copyright (c) 2024 Marc Morcos
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_
16+
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_
17+
18+
#include <vector>
19+
#include <memory>
20+
#include <string>
21+
22+
#include "geometry_msgs/msg/pose_stamped.hpp"
23+
#include "nav2_util/geometry_utils.hpp"
24+
#include "nav2_util/robot_utils.hpp"
25+
#include "behaviortree_cpp/action_node.h"
26+
#include "nav_msgs/msg/path.h"
27+
28+
namespace nav2_behavior_tree
29+
{
30+
31+
class GetPoseFromPath : public BT::ActionNodeBase
32+
{
33+
public:
34+
GetPoseFromPath(
35+
const std::string & xml_tag_name,
36+
const BT::NodeConfiguration & conf);
37+
38+
39+
static BT::PortsList providedPorts()
40+
{
41+
return {
42+
BT::InputPort<nav_msgs::msg::Path>("path", "Path to extract pose from"),
43+
BT::OutputPort<geometry_msgs::msg::PoseStamped>("pose", "Stamped Extracted Pose"),
44+
BT::InputPort<int>("index", 0, "Index of pose to extract from. -1 is end of list"),
45+
};
46+
}
47+
48+
private:
49+
void halt() override {}
50+
BT::NodeStatus tick() override;
51+
};
52+
53+
} // namespace nav2_behavior_tree
54+
55+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_

‎nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
// Copyright (c) 2024 Marc Morcos
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <string>
16+
#include <memory>
17+
#include <limits>
18+
19+
#include "nav_msgs/msg/path.hpp"
20+
#include "nav2_util/geometry_utils.hpp"
21+
22+
#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp"
23+
24+
namespace nav2_behavior_tree
25+
{
26+
27+
GetPoseFromPath::GetPoseFromPath(
28+
const std::string & name,
29+
const BT::NodeConfiguration & conf)
30+
: BT::ActionNodeBase(name, conf)
31+
{
32+
}
33+
34+
inline BT::NodeStatus GetPoseFromPath::tick()
35+
{
36+
setStatus(BT::NodeStatus::RUNNING);
37+
38+
nav_msgs::msg::Path input_path;
39+
getInput("path", input_path);
40+
41+
int pose_index;
42+
getInput("index", pose_index);
43+
44+
if (input_path.poses.empty()) {
45+
return BT::NodeStatus::FAILURE;
46+
}
47+
48+
// Account for negative indices
49+
if(pose_index < 0) {
50+
pose_index = input_path.poses.size() + pose_index;
51+
}
52+
53+
// out of bounds index
54+
if(pose_index < 0 || static_cast<unsigned>(pose_index) >= input_path.poses.size()) {
55+
return BT::NodeStatus::FAILURE;
56+
}
57+
58+
// extract pose
59+
geometry_msgs::msg::PoseStamped output_pose;
60+
output_pose = input_path.poses[pose_index];
61+
62+
// populate pose frame from path if necessary
63+
if(output_pose.header.frame_id.empty()) {
64+
output_pose.header.frame_id = input_path.header.frame_id;
65+
}
66+
67+
68+
setOutput("pose", output_pose);
69+
70+
return BT::NodeStatus::SUCCESS;
71+
}
72+
73+
} // namespace nav2_behavior_tree
74+
75+
#include "behaviortree_cpp/bt_factory.h"
76+
BT_REGISTER_NODES(factory)
77+
{
78+
factory.registerNodeType<nav2_behavior_tree::GetPoseFromPath>("GetPoseFromPath");
79+
}

‎nav2_behavior_tree/test/plugins/action/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,10 @@ ament_add_gtest(test_remove_passed_goals_action test_remove_passed_goals_action.
9292
target_link_libraries(test_remove_passed_goals_action nav2_remove_passed_goals_action_bt_node)
9393
ament_target_dependencies(test_remove_passed_goals_action ${dependencies})
9494

95+
ament_add_gtest(test_get_pose_from_path_action test_get_pose_from_path_action.cpp)
96+
target_link_libraries(test_get_pose_from_path_action nav2_get_pose_from_path_action_bt_node)
97+
ament_target_dependencies(test_get_pose_from_path_action ${dependencies})
98+
9599
ament_add_gtest(test_planner_selector_node test_planner_selector_node.cpp)
96100
target_link_libraries(test_planner_selector_node nav2_planner_selector_bt_node)
97101
ament_target_dependencies(test_planner_selector_node ${dependencies})

‎nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp‎

Lines changed: 156 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,156 @@
1+
// Copyright (c) 2018 Intel Corporation
2+
// Copyright (c) 2021 Samsung Research America
3+
// Copyright (c) 2024 Marc Morcos
4+
//
5+
// Licensed under the Apache License, Version 2.0 (the "License");
6+
// you may not use this file except in compliance with the License.
7+
// You may obtain a copy of the License at
8+
//
9+
// http://www.apache.org/licenses/LICENSE-2.0
10+
//
11+
// Unless required by applicable law or agreed to in writing, software
12+
// distributed under the License is distributed on an "AS IS" BASIS,
13+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
// See the License for the specific language governing permissions and
15+
// limitations under the License.
16+
17+
#include <gtest/gtest.h>
18+
#include <memory>
19+
#include <set>
20+
#include <string>
21+
#include <vector>
22+
23+
#include "nav_msgs/msg/path.hpp"
24+
#include "geometry_msgs/msg/pose_stamped.hpp"
25+
26+
#include "behaviortree_cpp/bt_factory.h"
27+
28+
#include "utils/test_action_server.hpp"
29+
#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp"
30+
#include "utils/test_behavior_tree_fixture.hpp"
31+
32+
class GetPoseFromPathTestFixture : public ::testing::Test
33+
{
34+
public:
35+
static void SetUpTestCase()
36+
{
37+
node_ = std::make_shared<rclcpp::Node>("get_pose_from_path_action_test_fixture");
38+
factory_ = std::make_shared<BT::BehaviorTreeFactory>();
39+
40+
config_ = new BT::NodeConfiguration();
41+
42+
// Create the blackboard that will be shared by all of the nodes in the tree
43+
config_->blackboard = BT::Blackboard::create();
44+
// Put items on the blackboard
45+
config_->blackboard->set<rclcpp::Node::SharedPtr>(
46+
"node",
47+
node_);
48+
49+
BT::NodeBuilder builder =
50+
[](const std::string & name, const BT::NodeConfiguration & config)
51+
{
52+
return std::make_unique<nav2_behavior_tree::GetPoseFromPath>(
53+
name, config);
54+
};
55+
56+
factory_->registerBuilder<nav2_behavior_tree::GetPoseFromPath>(
57+
"GetPoseFromPath", builder);
58+
}
59+
60+
static void TearDownTestCase()
61+
{
62+
delete config_;
63+
config_ = nullptr;
64+
node_.reset();
65+
factory_.reset();
66+
}
67+
68+
void TearDown() override
69+
{
70+
tree_.reset();
71+
}
72+
73+
protected:
74+
static rclcpp::Node::SharedPtr node_;
75+
static BT::NodeConfiguration * config_;
76+
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
77+
static std::shared_ptr<BT::Tree> tree_;
78+
};
79+
80+
rclcpp::Node::SharedPtr GetPoseFromPathTestFixture::node_ = nullptr;
81+
BT::NodeConfiguration * GetPoseFromPathTestFixture::config_ = nullptr;
82+
std::shared_ptr<BT::BehaviorTreeFactory> GetPoseFromPathTestFixture::factory_ = nullptr;
83+
std::shared_ptr<BT::Tree> GetPoseFromPathTestFixture::tree_ = nullptr;
84+
85+
TEST_F(GetPoseFromPathTestFixture, test_tick)
86+
{
87+
// create tree
88+
std::string xml_txt =
89+
R"(
90+
<root BTCPP_format="4">
91+
<BehaviorTree ID="MainTree">
92+
<GetPoseFromPath path="{path}" pose="{pose}" index="{index}" />
93+
</BehaviorTree>
94+
</root>)";
95+
96+
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
97+
98+
// create new path and set it on blackboard
99+
nav_msgs::msg::Path path;
100+
std::vector<geometry_msgs::msg::PoseStamped> goals;
101+
goals.resize(2);
102+
goals[0].pose.position.x = 1.0;
103+
goals[1].pose.position.x = 2.0;
104+
path.poses = goals;
105+
path.header.frame_id = "test_frame_1";
106+
config_->blackboard->set("path", path);
107+
108+
config_->blackboard->set("index", 0);
109+
110+
// tick until node succeeds
111+
while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
112+
tree_->rootNode()->executeTick();
113+
}
114+
115+
// the goal should have reached our server
116+
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
117+
118+
// check if returned pose is correct
119+
geometry_msgs::msg::PoseStamped pose;
120+
EXPECT_TRUE(config_->blackboard->get<geometry_msgs::msg::PoseStamped>("pose", pose));
121+
EXPECT_EQ(pose.header.frame_id, "test_frame_1");
122+
EXPECT_EQ(pose.pose.position.x, 1.0);
123+
124+
// halt node so another goal can be sent
125+
tree_->haltTree();
126+
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
127+
128+
// try last element
129+
config_->blackboard->set("index", -1);
130+
131+
while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
132+
tree_->rootNode()->executeTick();
133+
}
134+
135+
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
136+
137+
// check if returned pose is correct
138+
EXPECT_TRUE(config_->blackboard->get<geometry_msgs::msg::PoseStamped>("pose", pose));
139+
EXPECT_EQ(pose.header.frame_id, "test_frame_1");
140+
EXPECT_EQ(pose.pose.position.x, 2.0);
141+
}
142+
143+
int main(int argc, char ** argv)
144+
{
145+
::testing::InitGoogleTest(&argc, argv);
146+
147+
// initialize ROS
148+
rclcpp::init(argc, argv);
149+
150+
int all_successful = RUN_ALL_TESTS();
151+
152+
// shutdown ROS
153+
rclcpp::shutdown();
154+
155+
return all_successful;
156+
}

0 commit comments

Comments
 (0)