Skip to content

Commit

Permalink
Remove in collision goals (ros-navigation#4595)
Browse files Browse the repository at this point in the history
* Remove goals in collision

Signed-off-by: Tony Najjar <[email protected]>

* fix for main

Signed-off-by: Tony Najjar <[email protected]>

* Fix linting

Signed-off-by: Tony Najjar <[email protected]>

* linting fixes

Signed-off-by: Tony Najjar <[email protected]>

* PR fixes

Signed-off-by: Tony Najjar <[email protected]>

* remove second costmap

Signed-off-by: Tony Najjar <[email protected]>

* Minor fixes

Signed-off-by: Tony Najjar <[email protected]>

* getcosts instead of cost

Signed-off-by: Tony Najjar <[email protected]>

* get future once

Signed-off-by: Tony Najjar <[email protected]>

* replace with BTServiceNode

Signed-off-by: Tony Najjar <[email protected]>

* Add test

Signed-off-by: Tony Najjar <[email protected]>

* fix costmap tool

Signed-off-by: Tony Najjar <[email protected]>

* fix ament_uncrustify

Signed-off-by: Tony Najjar <[email protected]>

* remove duplicate

Signed-off-by: Tony Najjar <[email protected]>

* add return

Signed-off-by: Tony Najjar <[email protected]>

* fix typo

Signed-off-by: Tony Najjar <[email protected]>

* reset request before sending

Signed-off-by: Tony Najjar <[email protected]>

* pr comments fixes

Signed-off-by: Tony Najjar <[email protected]>

* fix ament cmake

Signed-off-by: Tony Najjar <[email protected]>

* fix test

Signed-off-by: Tony Najjar <[email protected]>

* fix

Signed-off-by: Tony Najjar <[email protected]>

---------

Signed-off-by: Tony Najjar <[email protected]>
Signed-off-by: Joseph Duchesne <[email protected]>
  • Loading branch information
tonynajjar authored and josephduchesne committed Dec 10, 2024
1 parent b0e9e36 commit 5a2530e
Show file tree
Hide file tree
Showing 15 changed files with 421 additions and 65 deletions.
3 changes: 3 additions & 0 deletions nav2_behavior_tree/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -176,6 +176,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_remove_in_collision_goals_action_bt_node SHARED plugins/action/remove_in_collision_goals_action.cpp)
list(APPEND plugin_libs nav2_remove_in_collision_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)

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
// Copyright (c) 2024 Angsa Robotics
//
// 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__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_

#include <vector>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_behavior_tree/bt_service_node.hpp"
#include "nav2_msgs/srv/get_costs.hpp"

namespace nav2_behavior_tree
{

class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
{
public:
typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;

/**
* @brief A constructor for nav2_behavior_tree::RemoveInCollisionGoals
* @param service_node_name Service name this node creates a client for
* @param conf BT node configuration
*/
RemoveInCollisionGoals(
const std::string & service_node_name,
const BT::NodeConfiguration & conf);

/**
* @brief The main override required by a BT service
* @return BT::NodeStatus Status of tick execution
*/
void on_tick() override;

BT::NodeStatus on_completion(std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
override;

static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
BT::InputPort<double>("cost_threshold", 254.0,
"Cost threshold for considering a goal in collision"),
BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
});
}

private:
bool use_footprint_;
double cost_threshold_;
Goals input_goals_;
};

} // namespace nav2_behavior_tree

#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_
8 changes: 8 additions & 0 deletions nav2_behavior_tree/nav2_tree_nodes.xml
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,14 @@
<output_port name="output_goals">Set of goals after removing any passed</output_port>
</Action>

<Action ID="RemoveInCollisionGoals">
<input_port name="service_name">Costmap service name responsible for getting the cost</input_port>
<input_port name="input_goals">A vector of goals to check if in collision</input_port>
<input_port name="use_footprint">Whether to use the footprint cost or the point cost.</input_port>
<input_port name="cost_threshold">The cost threshold above which a waypoint is considered in collision and should be removed.</input_port>
<output_port name="output_goals">A vector of goals containing only those that are not in collision.</output_port>
</Action>

<Action ID="SmoothPath">
<input_port name="smoother_id" default="SmoothPath"/>
<input_port name="unsmoothed_path">Path to be smoothed</input_port>
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,77 @@
// Copyright (c) 2024 Angsa Robotics
//
// 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 "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp"
#include "nav2_behavior_tree/bt_utils.hpp"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

namespace nav2_behavior_tree
{

RemoveInCollisionGoals::RemoveInCollisionGoals(
const std::string & service_node_name,
const BT::NodeConfiguration & conf)
: BtServiceNode<nav2_msgs::srv::GetCosts>(service_node_name, conf,
"/global_costmap/get_cost_global_costmap")
{}


void RemoveInCollisionGoals::on_tick()
{
getInput("use_footprint", use_footprint_);
getInput("cost_threshold", cost_threshold_);
getInput("input_goals", input_goals_);

if (input_goals_.empty()) {
setOutput("output_goals", input_goals_);
should_send_request_ = false;
return;
}
request_ = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
request_->use_footprint = use_footprint_;

for (const auto & goal : input_goals_) {
geometry_msgs::msg::Pose2D pose;
pose.x = goal.pose.position.x;
pose.y = goal.pose.position.y;
pose.theta = tf2::getYaw(goal.pose.orientation);
request_->poses.push_back(pose);
}
}

BT::NodeStatus RemoveInCollisionGoals::on_completion(
std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
{
Goals valid_goal_poses;
for (size_t i = 0; i < response->costs.size(); ++i) {
if (response->costs[i] < cost_threshold_) {
valid_goal_poses.push_back(input_goals_[i]);
}
}
setOutput("output_goals", valid_goal_poses);
return BT::NodeStatus::SUCCESS;
}

} // namespace nav2_behavior_tree

#include "behaviortree_cpp/bt_factory.h"
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<nav2_behavior_tree::RemoveInCollisionGoals>("RemoveInCollisionGoals");
}
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ void RemovePassedGoals::initialize()

robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
node, "robot_base_frame", this);
initialized_ = true;
}

inline BT::NodeStatus RemovePassedGoals::tick()
Expand Down
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 @@ -51,6 +51,10 @@ plugin_add_test(test_truncate_path_local_action test_truncate_path_local_action.

plugin_add_test(test_remove_passed_goals_action test_remove_passed_goals_action.cpp nav2_remove_passed_goals_action_bt_node)

plugin_add_test(test_remove_in_collision_goals_action
test_remove_in_collision_goals_action.cpp
nav2_remove_in_collision_goals_action_bt_node)

plugin_add_test(test_get_pose_from_path_action test_get_pose_from_path_action.cpp nav2_get_pose_from_path_action_bt_node)

plugin_add_test(test_planner_selector_node test_planner_selector_node.cpp nav2_planner_selector_bt_node)
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,180 @@
// Copyright (c) 2024 Angsa Robotics
//
// 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 "behaviortree_cpp/bt_factory.h"

#include "utils/test_service.hpp"
#include "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp"
#include "utils/test_behavior_tree_fixture.hpp"


class RemoveInCollisionGoalsService : public TestService<nav2_msgs::srv::GetCosts>
{
public:
RemoveInCollisionGoalsService()
: TestService("/global_costmap/get_cost_global_costmap")
{}

virtual void handle_service(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<nav2_msgs::srv::GetCosts::Request> request,
const std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
{
(void)request_header;
(void)request;
response->costs = {100, 50, 5, 254};
}
};


class RemoveInCollisionGoalsTestFixture : public ::testing::Test
{
public:
static void SetUpTestCase()
{
node_ = std::make_shared<rclcpp::Node>("in_collision_goals_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(
"node",
node_);
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds(20));
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));

BT::NodeBuilder builder =
[](const std::string & name, const BT::NodeConfiguration & config)
{
return std::make_unique<nav2_behavior_tree::RemoveInCollisionGoals>(
name, config);
};

factory_->registerBuilder<nav2_behavior_tree::RemoveInCollisionGoals>(
"RemoveInCollisionGoals", builder);
}

static void TearDownTestCase()
{
delete config_;
config_ = nullptr;
node_.reset();
server_.reset();
factory_.reset();
}

void TearDown() override
{
tree_.reset();
}
static std::shared_ptr<RemoveInCollisionGoalsService> server_;

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 RemoveInCollisionGoalsTestFixture::node_ = nullptr;

BT::NodeConfiguration * RemoveInCollisionGoalsTestFixture::config_ = nullptr;
std::shared_ptr<RemoveInCollisionGoalsService>
RemoveInCollisionGoalsTestFixture::server_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> RemoveInCollisionGoalsTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> RemoveInCollisionGoalsTestFixture::tree_ = nullptr;

TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals)
{
// create tree
std::string xml_txt =
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<RemoveInCollisionGoals service_name="/global_costmap/get_cost_global_costmap" input_goals="{goals}" output_goals="{goals}" cost_threshold="253"/>
</BehaviorTree>
</root>)";

tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));

// create new goal and set it on blackboard
std::vector<geometry_msgs::msg::PoseStamped> poses;
poses.resize(4);
poses[0].pose.position.x = 0.0;
poses[0].pose.position.y = 0.0;

poses[1].pose.position.x = 0.5;
poses[1].pose.position.y = 0.0;

poses[2].pose.position.x = 1.0;
poses[2].pose.position.y = 0.0;

poses[3].pose.position.x = 2.0;
poses[3].pose.position.y = 0.0;

config_->blackboard->set("goals", poses);

// tick until node succeeds
while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
tree_->rootNode()->executeTick();
}

// check that it removed the point in range
std::vector<geometry_msgs::msg::PoseStamped> output_poses;
EXPECT_TRUE(config_->blackboard->get("goals", output_poses));

EXPECT_EQ(output_poses.size(), 3u);
EXPECT_EQ(output_poses[0], poses[0]);
EXPECT_EQ(output_poses[1], poses[1]);
EXPECT_EQ(output_poses[2], poses[2]);
}

int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);

// initialize ROS
rclcpp::init(argc, argv);

// initialize service and spin on new thread
RemoveInCollisionGoalsTestFixture::server_ =
std::make_shared<RemoveInCollisionGoalsService>();
std::thread server_thread([]() {
rclcpp::spin(RemoveInCollisionGoalsTestFixture::server_);
});

int all_successful = RUN_ALL_TESTS();

// shutdown ROS
rclcpp::shutdown();
server_thread.join();

return all_successful;
}
Loading

0 comments on commit 5a2530e

Please sign in to comment.