Skip to content

Commit c0bf67e

Browse files
SteveMacenskiSteveMacenski
authored
Adding clear costmap around pose service option (#5309)
* Adding clear costmap around pose impl Signed-off-by: Steve Macenski <[email protected]> * Update nav2_msgs/srv/ClearCostmapAroundPose.srv Signed-off-by: Steve Macenski <[email protected]> * Adding APIs for simple commander Signed-off-by: Steve Macenski <[email protected]> * linting Signed-off-by: Steve Macenski <[email protected]> * adding import Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]>
1 parent fa63e30 commit c0bf67e

10 files changed

Lines changed: 282 additions & 2 deletions

File tree

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

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include "nav2_behavior_tree/bt_service_node.hpp"
2121
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
2222
#include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
23+
#include "nav2_msgs/srv/clear_costmap_around_pose.hpp"
2324
#include "nav2_msgs/srv/clear_costmap_except_region.hpp"
2425
#include "nav2_ros_common/lifecycle_node.hpp"
2526

@@ -129,6 +130,47 @@ class ClearCostmapAroundRobotService : public BtServiceNode<nav2_msgs::srv::Clea
129130
}
130131
};
131132

133+
/**
134+
* @brief A nav2_behavior_tree::BtServiceNode class that
135+
* wraps nav2_msgs::srv::ClearCostmapAroundPose
136+
* @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
137+
* It will re-initialize when halted.
138+
*/
139+
class ClearCostmapAroundPoseService : public BtServiceNode<nav2_msgs::srv::ClearCostmapAroundPose>
140+
{
141+
public:
142+
/**
143+
* @brief A constructor for nav2_behavior_tree::ClearCostmapAroundPoseService
144+
* @param service_node_name Service name this node creates a client for
145+
* @param conf BT node configuration
146+
*/
147+
ClearCostmapAroundPoseService(
148+
const std::string & service_node_name,
149+
const BT::NodeConfiguration & conf);
150+
151+
/**
152+
* @brief The main override required by a BT service
153+
* @return BT::NodeStatus Status of tick execution
154+
*/
155+
void on_tick() override;
156+
157+
/**
158+
* @brief Creates list of BT ports
159+
* @return BT::PortsList Containing basic ports along with node-specific ports
160+
*/
161+
static BT::PortsList providedPorts()
162+
{
163+
return providedBasicPorts(
164+
{
165+
BT::InputPort<geometry_msgs::msg::PoseStamped>(
166+
"pose", "Pose around which to clear the costmap"),
167+
BT::InputPort<double>(
168+
"reset_distance", 1.0,
169+
"Distance from the pose under which obstacles are cleared")
170+
});
171+
}
172+
};
173+
132174
} // namespace nav2_behavior_tree
133175

134176
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_

‎nav2_behavior_tree/nav2_tree_nodes.xml‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,13 @@
8181
<input_port name="reset_distance">Distance from the robot under which obstacles are cleared</input_port>
8282
</Action>
8383

84+
<Action ID="ClearCostmapAroundPose">
85+
<input_port name="service_name">Service name</input_port>
86+
<input_port name="server_timeout">Server timeout</input_port>
87+
<input_port name="pose">Pose around which to clear the costmap</input_port>
88+
<input_port name="reset_distance">Distance from the pose under which obstacles are cleared</input_port>
89+
</Action>
90+
8491
<Action ID="ComputePathToPose">
8592
<input_port name="goal">Destination to plan to</input_port>
8693
<input_port name="start">Use as the planner start pose instead of the current robot pose, if use_start is not false (i.e. not provided or set to true)</input_port>

‎nav2_behavior_tree/plugins/action/clear_costmap_service.cpp‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,20 @@ void ClearCostmapAroundRobotService::on_tick()
5858
increment_recovery_count();
5959
}
6060

61+
ClearCostmapAroundPoseService::ClearCostmapAroundPoseService(
62+
const std::string & service_node_name,
63+
const BT::NodeConfiguration & conf)
64+
: BtServiceNode<nav2_msgs::srv::ClearCostmapAroundPose>(service_node_name, conf)
65+
{
66+
}
67+
68+
void ClearCostmapAroundPoseService::on_tick()
69+
{
70+
getInput("pose", request_->pose);
71+
getInput("reset_distance", request_->reset_distance);
72+
increment_recovery_count();
73+
}
74+
6175
} // namespace nav2_behavior_tree
6276

6377
#include "behaviortree_cpp/bt_factory.h"
@@ -68,4 +82,6 @@ BT_REGISTER_NODES(factory)
6882
"ClearCostmapExceptRegion");
6983
factory.registerNodeType<nav2_behavior_tree::ClearCostmapAroundRobotService>(
7084
"ClearCostmapAroundRobot");
85+
factory.registerNodeType<nav2_behavior_tree::ClearCostmapAroundPoseService>(
86+
"ClearCostmapAroundPose");
7187
}

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

Lines changed: 102 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -302,6 +302,101 @@ TEST_F(ClearCostmapAroundRobotServiceTestFixture, test_tick)
302302
EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
303303
}
304304

305+
class ClearCostmapAroundPoseService : public TestService<nav2_msgs::srv::ClearCostmapAroundPose>
306+
{
307+
public:
308+
ClearCostmapAroundPoseService()
309+
: TestService("clear_costmap_around_pose")
310+
{}
311+
};
312+
313+
class ClearCostmapAroundPoseServiceTestFixture : public ::testing::Test
314+
{
315+
public:
316+
static void SetUpTestCase()
317+
{
318+
node_ = std::make_shared<nav2::LifecycleNode>("clear_costmap_around_pose_test_fixture");
319+
factory_ = std::make_shared<BT::BehaviorTreeFactory>();
320+
321+
config_ = new BT::NodeConfiguration();
322+
323+
// Create the blackboard that will be shared by all of the nodes in the tree
324+
config_->blackboard = BT::Blackboard::create();
325+
// Put items on the blackboard
326+
config_->blackboard->set(
327+
"node",
328+
node_);
329+
config_->blackboard->set<std::chrono::milliseconds>(
330+
"server_timeout",
331+
std::chrono::milliseconds(10));
332+
config_->blackboard->set<std::chrono::milliseconds>(
333+
"bt_loop_duration",
334+
std::chrono::milliseconds(10));
335+
config_->blackboard->set<std::chrono::milliseconds>(
336+
"wait_for_service_timeout",
337+
std::chrono::milliseconds(1000));
338+
config_->blackboard->set("initial_pose_received", false);
339+
config_->blackboard->set("number_recoveries", 0);
340+
341+
factory_->registerNodeType<nav2_behavior_tree::ClearCostmapAroundPoseService>(
342+
"ClearCostmapAroundPose");
343+
}
344+
345+
static void TearDownTestCase()
346+
{
347+
delete config_;
348+
config_ = nullptr;
349+
node_.reset();
350+
server_.reset();
351+
factory_.reset();
352+
}
353+
354+
void SetUp() override
355+
{
356+
config_->blackboard->set("number_recoveries", 0);
357+
}
358+
359+
void TearDown() override
360+
{
361+
tree_.reset();
362+
}
363+
364+
static std::shared_ptr<ClearCostmapAroundPoseService> server_;
365+
366+
protected:
367+
static nav2::LifecycleNode::SharedPtr node_;
368+
static BT::NodeConfiguration * config_;
369+
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
370+
static std::shared_ptr<BT::Tree> tree_;
371+
};
372+
373+
nav2::LifecycleNode::SharedPtr
374+
ClearCostmapAroundPoseServiceTestFixture::node_ = nullptr;
375+
std::shared_ptr<ClearCostmapAroundPoseService>
376+
ClearCostmapAroundPoseServiceTestFixture::server_ = nullptr;
377+
BT::NodeConfiguration
378+
* ClearCostmapAroundPoseServiceTestFixture::config_ = nullptr;
379+
std::shared_ptr<BT::BehaviorTreeFactory>
380+
ClearCostmapAroundPoseServiceTestFixture::factory_ = nullptr;
381+
std::shared_ptr<BT::Tree>
382+
ClearCostmapAroundPoseServiceTestFixture::tree_ = nullptr;
383+
384+
TEST_F(ClearCostmapAroundPoseServiceTestFixture, test_tick)
385+
{
386+
std::string xml_txt =
387+
R"(
388+
<root BTCPP_format="4">
389+
<BehaviorTree ID="MainTree">
390+
<ClearCostmapAroundPose service_name="clear_costmap_around_pose"/>
391+
</BehaviorTree>
392+
</root>)";
393+
394+
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
395+
EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
396+
EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
397+
EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
398+
}
399+
305400
int main(int argc, char ** argv)
306401
{
307402
::testing::InitGoogleTest(&argc, argv);
@@ -327,13 +422,20 @@ int main(int argc, char ** argv)
327422
rclcpp::spin(ClearCostmapAroundRobotServiceTestFixture::server_);
328423
});
329424

425+
ClearCostmapAroundPoseServiceTestFixture::server_ =
426+
std::make_shared<ClearCostmapAroundPoseService>();
427+
std::thread server_thread_around_pose([]() {
428+
rclcpp::spin(ClearCostmapAroundPoseServiceTestFixture::server_);
429+
});
430+
330431
int all_successful = RUN_ALL_TESTS();
331432

332433
// shutdown ROS
333434
rclcpp::shutdown();
334435
server_thread.join();
335436
server_thread_except_region.join();
336437
server_thread_around_robot.join();
438+
server_thread_around_pose.join();
337439

338440
return all_successful;
339441
}

‎nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include "rclcpp/rclcpp.hpp"
2323
#include "nav2_msgs/srv/clear_costmap_except_region.hpp"
2424
#include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
25+
#include "nav2_msgs/srv/clear_costmap_around_pose.hpp"
2526
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
2627
#include "nav2_costmap_2d/costmap_layer.hpp"
2728
#include "nav2_ros_common/lifecycle_node.hpp"
@@ -60,6 +61,11 @@ class ClearCostmapService
6061
*/
6162
void clearRegion(double reset_distance, bool invert);
6263

64+
/**
65+
* @brief Clears the region around a specific pose
66+
*/
67+
void clearAroundPose(const geometry_msgs::msg::PoseStamped & pose, double reset_distance);
68+
6369
/**
6470
* @brief Clears all layers
6571
*/
@@ -96,6 +102,16 @@ class ClearCostmapService
96102
const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Request> request,
97103
const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Response> response);
98104

105+
nav2::ServiceServer<nav2_msgs::srv::ClearCostmapAroundPose>::SharedPtr
106+
clear_around_pose_service_;
107+
/**
108+
* @brief Callback to clear costmap around a given pose
109+
*/
110+
void clearAroundPoseCallback(
111+
const std::shared_ptr<rmw_request_id_t> request_header,
112+
const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundPose::Request> request,
113+
const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundPose::Response> response);
114+
99115
nav2::ServiceServer<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr
100116
clear_entire_service_;
101117
/**

‎nav2_costmap_2d/src/clear_costmap_service.cpp‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ using std::shared_ptr;
2929
using std::any_of;
3030
using ClearExceptRegion = nav2_msgs::srv::ClearCostmapExceptRegion;
3131
using ClearAroundRobot = nav2_msgs::srv::ClearCostmapAroundRobot;
32+
using ClearAroundPose = nav2_msgs::srv::ClearCostmapAroundPose;
3233
using ClearEntirely = nav2_msgs::srv::ClearEntireCostmap;
3334

3435
ClearCostmapService::ClearCostmapService(
@@ -52,6 +53,12 @@ ClearCostmapService::ClearCostmapService(
5253
&ClearCostmapService::clearAroundRobotCallback, this,
5354
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
5455

56+
clear_around_pose_service_ = node->create_service<ClearAroundPose>(
57+
std::string("clear_around_pose_") + costmap_.getName(),
58+
std::bind(
59+
&ClearCostmapService::clearAroundPoseCallback, this,
60+
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
61+
5562
clear_entire_service_ = node->create_service<ClearEntirely>(
5663
std::string("clear_entirely_") + costmap_.getName(),
5764
std::bind(
@@ -64,6 +71,7 @@ ClearCostmapService::~ClearCostmapService()
6471
// make sure services shutdown.
6572
clear_except_service_.reset();
6673
clear_around_service_.reset();
74+
clear_around_pose_service_.reset();
6775
clear_entire_service_.reset();
6876
}
6977

@@ -87,6 +95,18 @@ void ClearCostmapService::clearAroundRobotCallback(
8795
clearRegion(request->reset_distance, false);
8896
}
8997

98+
void ClearCostmapService::clearAroundPoseCallback(
99+
const shared_ptr<rmw_request_id_t>/*request_header*/,
100+
const shared_ptr<ClearAroundPose::Request> request,
101+
const shared_ptr<ClearAroundPose::Response>/*response*/)
102+
{
103+
RCLCPP_INFO(
104+
logger_, "%s",
105+
("Received request to clear around pose for " + costmap_.getName()).c_str());
106+
107+
clearAroundPose(request->pose, request->reset_distance);
108+
}
109+
90110
void ClearCostmapService::clearEntireCallback(
91111
const std::shared_ptr<rmw_request_id_t>/*request_header*/,
92112
const std::shared_ptr<ClearEntirely::Request>/*request*/,
@@ -99,6 +119,41 @@ void ClearCostmapService::clearEntireCallback(
99119
clearEntirely();
100120
}
101121

122+
void ClearCostmapService::clearAroundPose(
123+
const geometry_msgs::msg::PoseStamped & pose,
124+
const double reset_distance)
125+
{
126+
double x, y;
127+
128+
// Transform pose to costmap frame if necessary
129+
geometry_msgs::msg::PoseStamped global_pose;
130+
try {
131+
if (pose.header.frame_id == costmap_.getGlobalFrameID()) {
132+
global_pose = pose;
133+
} else {
134+
costmap_.getTfBuffer()->transform(pose, global_pose, costmap_.getGlobalFrameID());
135+
}
136+
} catch (tf2::TransformException & ex) {
137+
RCLCPP_ERROR(
138+
logger_,
139+
"Cannot clear map around pose because pose cannot be transformed to costmap frame: %s",
140+
ex.what());
141+
return;
142+
}
143+
144+
x = global_pose.pose.position.x;
145+
y = global_pose.pose.position.y;
146+
147+
auto layers = costmap_.getLayeredCostmap()->getPlugins();
148+
149+
for (auto & layer : *layers) {
150+
if (layer->isClearable()) {
151+
auto costmap_layer = std::static_pointer_cast<CostmapLayer>(layer);
152+
clearLayerRegion(costmap_layer, x, y, reset_distance, false);
153+
}
154+
}
155+
}
156+
102157
void ClearCostmapService::clearRegion(const double reset_distance, bool invert)
103158
{
104159
double x, y;

‎nav2_msgs/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
4343
"srv/IsPathValid.srv"
4444
"srv/ClearCostmapExceptRegion.srv"
4545
"srv/ClearCostmapAroundRobot.srv"
46+
"srv/ClearCostmapAroundPose.srv"
4647
"srv/ClearEntireCostmap.srv"
4748
"srv/ManageLifecycleNodes.srv"
4849
"srv/LoadMap.srv"

‎nav2_msgs/srv/ClearCostmapAroundPose.srv‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
# Clears the costmap within a distance around a given pose
2+
3+
geometry_msgs/PoseStamped pose
4+
float64 reset_distance
5+
---
6+
std_msgs/Empty response

‎nav2_msgs/srv/__init__.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
from nav2_msgs.srv._clear_costmap_around_pose import ClearCostmapAroundPose
12
from nav2_msgs.srv._clear_costmap_around_robot import ClearCostmapAroundRobot
23
from nav2_msgs.srv._clear_costmap_except_region import ClearCostmapExceptRegion
34
from nav2_msgs.srv._clear_entire_costmap import ClearEntireCostmap
@@ -13,6 +14,7 @@
1314
__all__ = [
1415
'ClearCostmapAroundRobot',
1516
'ClearCostmapExceptRegion',
17+
'ClearCostmapAroundPose',
1618
'ClearEntireCostmap',
1719
'GetCostmap',
1820
'GetCosts',

0 commit comments

Comments
 (0)