Skip to content

Commit 645d09a

Browse files
mini-1235mini-1235
authored
Add isPoseOccupied BT node (#5556)
* Add isPoseOccupied BT node Signed-off-by: mini-1235 <[email protected]> * Remove cout Signed-off-by: mini-1235 <[email protected]> --------- Signed-off-by: mini-1235 <[email protected]>
1 parent 4e5273a commit 645d09a

File tree

6 files changed

+401
-0
lines changed

6 files changed

+401
-0
lines changed

‎nav2_behavior_tree/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -120,6 +120,9 @@ list(APPEND plugin_libs nav2_goal_updated_condition_bt_node)
120120
add_library(nav2_is_path_valid_condition_bt_node SHARED plugins/condition/is_path_valid_condition.cpp)
121121
list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node)
122122

123+
add_library(nav2_is_pose_occupied_condition_bt_node SHARED plugins/condition/is_pose_occupied_condition.cpp)
124+
list(APPEND plugin_libs nav2_is_pose_occupied_condition_bt_node)
125+
123126
add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp)
124127
list(APPEND plugin_libs nav2_time_expired_condition_bt_node)
125128

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
// Copyright (c) 2025 Maurice Alexander Purnawan
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__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_
16+
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_
17+
18+
#include <string>
19+
#include <memory>
20+
21+
#include "nav2_ros_common/lifecycle_node.hpp"
22+
#include "behaviortree_cpp/condition_node.h"
23+
#include "behaviortree_cpp/json_export.h"
24+
#include "geometry_msgs/msg/pose_stamped.hpp"
25+
#include "nav2_msgs/srv/get_costs.hpp"
26+
#include "nav2_ros_common/service_client.hpp"
27+
#include "nav2_behavior_tree/bt_utils.hpp"
28+
#include "nav2_behavior_tree/json_utils.hpp"
29+
30+
31+
namespace nav2_behavior_tree
32+
{
33+
34+
/**
35+
* @brief A BT::ConditionNode that returns SUCCESS when the IsPoseOccupied
36+
* service returns true and FAILURE otherwise
37+
*/
38+
class IsPoseOccupiedCondition : public BT::ConditionNode
39+
{
40+
public:
41+
/**
42+
* @brief A constructor for nav2_behavior_tree::IsPoseOccupiedCondition
43+
* @param condition_name Name for the XML tag for this node
44+
* @param conf BT node configuration
45+
*/
46+
IsPoseOccupiedCondition(
47+
const std::string & condition_name,
48+
const BT::NodeConfiguration & conf);
49+
50+
IsPoseOccupiedCondition() = delete;
51+
52+
/**
53+
* @brief The main override required by a BT action
54+
* @return BT::NodeStatus Status of tick execution
55+
*/
56+
BT::NodeStatus tick() override;
57+
58+
/**
59+
* @brief Function to read parameters and initialize class variables
60+
*/
61+
void initialize();
62+
63+
/**
64+
* @brief Creates list of BT ports
65+
* @return BT::PortsList Containing node-specific ports
66+
*/
67+
static BT::PortsList providedPorts()
68+
{
69+
// Register JSON definitions for the types used in the ports
70+
BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
71+
BT::RegisterJsonDefinition<std::chrono::milliseconds>();
72+
73+
return {
74+
BT::InputPort<geometry_msgs::msg::PoseStamped>("pose", "Pose to check if occupied"),
75+
BT::InputPort<std::string>("service_name", "global_costmap/get_cost_global_costmap"),
76+
BT::InputPort<double>(
77+
"cost_threshold", 254.0,
78+
"Cost threshold for considering a pose occupied"),
79+
BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
80+
BT::InputPort<bool>(
81+
"consider_unknown_as_obstacle", false,
82+
"Whether to consider unknown cost as obstacle"),
83+
BT::InputPort<std::chrono::milliseconds>("server_timeout"),
84+
};
85+
}
86+
87+
private:
88+
nav2::LifecycleNode::SharedPtr node_;
89+
nav2::ServiceClient<nav2_msgs::srv::GetCosts>::SharedPtr client_;
90+
// The timeout value while waiting for a response from the
91+
// get cost service
92+
std::chrono::milliseconds server_timeout_;
93+
bool use_footprint_;
94+
bool consider_unknown_as_obstacle_;
95+
double cost_threshold_;
96+
std::string service_name_;
97+
};
98+
99+
} // namespace nav2_behavior_tree
100+
101+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_POSE_OCCUPIED_CONDITION_HPP_

‎nav2_behavior_tree/nav2_tree_nodes.xml‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -394,6 +394,15 @@
394394
<input_port name="consider_unknown_as_obstacle"> If unknown cost is considered valid </input_port>
395395
</Condition>
396396

397+
<Condition ID="IsPoseOccupied">
398+
<input_port name="pose">Pose to check whether it is occupied</input_port>
399+
<input_port name="service_name">Service name to call GetCosts</input_port>
400+
<input_port name="cost_threshold">The cost threshold above which a pose is considered occupied</input_port>
401+
<input_port name="use_footprint">Whether to use the footprint cost or the point cost</input_port>
402+
<input_port name="consider_unknown_as_obstacle"> If unknown cost is considered valid </input_port>
403+
<input_port name="server_timeout"> Server timeout </input_port>
404+
</Condition>
405+
397406
<Condition ID="WouldAControllerRecoveryHelp">
398407
<input_port name="error_code">Error code</input_port>
399408
</Condition>

‎nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
// Copyright (c) 2025 Maurice Alexander Purnawan
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 "nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp"
16+
#include <chrono>
17+
#include <memory>
18+
#include <string>
19+
20+
namespace nav2_behavior_tree
21+
{
22+
23+
IsPoseOccupiedCondition::IsPoseOccupiedCondition(
24+
const std::string & condition_name,
25+
const BT::NodeConfiguration & conf)
26+
: BT::ConditionNode(condition_name, conf),
27+
use_footprint_(true), consider_unknown_as_obstacle_(false), cost_threshold_(254),
28+
service_name_("global_costmap/get_cost_global_costmap")
29+
{
30+
node_ = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
31+
server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
32+
}
33+
34+
void IsPoseOccupiedCondition::initialize()
35+
{
36+
getInput<std::string>("service_name", service_name_);
37+
getInput<double>("cost_threshold", cost_threshold_);
38+
getInput<bool>("use_footprint", use_footprint_);
39+
getInput<bool>("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
40+
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
41+
client_ =
42+
node_->create_client<nav2_msgs::srv::GetCosts>(
43+
service_name_,
44+
false /* Does not create and spin an internal executor*/);
45+
}
46+
47+
BT::NodeStatus IsPoseOccupiedCondition::tick()
48+
{
49+
if (!BT::isStatusActive(status())) {
50+
initialize();
51+
}
52+
geometry_msgs::msg::PoseStamped pose;
53+
getInput("pose", pose);
54+
55+
auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
56+
request->use_footprint = use_footprint_;
57+
request->poses.push_back(pose);
58+
59+
auto response = client_->invoke(request, server_timeout_);
60+
61+
if(!response->success) {
62+
RCLCPP_ERROR(
63+
node_->get_logger(),
64+
"GetCosts service call failed");
65+
return BT::NodeStatus::FAILURE;
66+
}
67+
68+
if((response->costs[0] == 255 && !consider_unknown_as_obstacle_) ||
69+
response->costs[0] < cost_threshold_)
70+
{
71+
return BT::NodeStatus::FAILURE;
72+
} else {
73+
return BT::NodeStatus::SUCCESS;
74+
}
75+
}
76+
77+
} // namespace nav2_behavior_tree
78+
79+
#include "behaviortree_cpp/bt_factory.h"
80+
BT_REGISTER_NODES(factory)
81+
{
82+
factory.registerNodeType<nav2_behavior_tree::IsPoseOccupiedCondition>("IsPoseOccupied");
83+
}

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,8 @@ plugin_add_test(test_condition_is_battery_low test_is_battery_low.cpp nav2_is_ba
2626

2727
plugin_add_test(test_condition_is_path_valid test_is_path_valid.cpp nav2_is_path_valid_condition_bt_node)
2828

29+
plugin_add_test(test_condition_is_pose_occupied test_is_pose_occupied.cpp nav2_is_pose_occupied_condition_bt_node)
30+
2931
plugin_add_test(test_are_error_codes_present test_are_error_codes_present.cpp nav2_would_a_controller_recovery_help_condition_bt_node)
3032

3133
plugin_add_test(test_would_a_controller_recovery_help

0 commit comments

Comments
 (0)