Skip to content

Commit

Permalink
Behavior tree node for extracting pose from path (ros-navigation#4518)
Browse files Browse the repository at this point in the history
* add get pose from path action

Signed-off-by: MarcM0 <[email protected]>

* cleanup from PR suggestions

Signed-off-by: MarcM0 <[email protected]>

* Updates for main compatibility

Signed-off-by: MarcM0 <[email protected]>

* Lint and build fix

Signed-off-by: MarcM0 <[email protected]>

* More Lint and warnings

Signed-off-by: MarcM0 <[email protected]>

* More Lint and build

Signed-off-by: MarcM0 <[email protected]>

* remove code left over from older file

Signed-off-by: MarcM0 <[email protected]>

* fix test blackboard var name

Signed-off-by: MarcM0 <[email protected]>

* only populate pose frame if empty

Signed-off-by: MarcM0 <[email protected]>

* lint

Signed-off-by: MarcM0 <[email protected]>

---------

Signed-off-by: MarcM0 <[email protected]>
  • Loading branch information
Marc-Morcos authored Jul 5, 2024
1 parent 568f8ff commit 12a9c1d
Show file tree
Hide file tree
Showing 5 changed files with 297 additions and 0 deletions.
3 changes: 3 additions & 0 deletions nav2_behavior_tree/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
Original file line number Diff line number Diff line change
@@ -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_
79 changes: 79 additions & 0 deletions nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp
Original file line number Diff line number Diff line change
@@ -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");
}
4 changes: 4 additions & 0 deletions nav2_behavior_tree/test/plugins/action/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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})
Expand Down
Original file line number Diff line number Diff line change
@@ -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;
}

0 comments on commit 12a9c1d

Please sign in to comment.