Skip to content

Commit 417d41f

Browse files
tonynajjartonynajjar
committed
Publish planned footprints after smoothing (ros-navigation#5155)
* Publish planned footprints after smoothing Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "Publish planned footprints after smoothing" This reverts commit c9b349a. * Add smoothed footprints publishing Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix indentation for publisher creation in SmacPlannerHybrid and SmacPlannerLattice Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * address PR comments Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fixes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix build error Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
1 parent e167aaf commit 417d41f

File tree

4 files changed

+52
-45
lines changed

4 files changed

+52
-45
lines changed

‎nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -127,7 +127,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
127127
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
128128
_planned_footprints_publisher;
129129
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
130-
_planned_footprints_smoothed_publisher;
130+
_smoothed_footprints_publisher;
131131
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
132132
_expansions_publisher;
133133
std::mutex _mutex;

‎nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
119119
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
120120
_planned_footprints_publisher;
121121
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
122-
_planned_footprints_smoothed_publisher;
122+
_smoothed_footprints_publisher;
123123
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
124124
_expansions_publisher;
125125
std::mutex _mutex;

‎nav2_smac_planner/src/smac_planner_hybrid.cpp‎

Lines changed: 24 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -258,8 +258,9 @@ void SmacPlannerHybrid::configure(
258258
_expansions_publisher = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
259259
_planned_footprints_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
260260
"planned_footprints", 1);
261-
_planned_footprints_smoothed_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
262-
"planned_footprints_smoothed", 1);
261+
_smoothed_footprints_publisher =
262+
node->create_publisher<visualization_msgs::msg::MarkerArray>(
263+
"smoothed_footprints", 1);
263264
}
264265

265266
RCLCPP_INFO(
@@ -280,7 +281,7 @@ void SmacPlannerHybrid::activate()
280281
if (_debug_visualizations) {
281282
_expansions_publisher->on_activate();
282283
_planned_footprints_publisher->on_activate();
283-
_planned_footprints_smoothed_publisher->on_activate();
284+
_smoothed_footprints_publisher->on_activate();
284285
}
285286
if (_costmap_downsampler) {
286287
_costmap_downsampler->on_activate();
@@ -310,7 +311,7 @@ void SmacPlannerHybrid::deactivate()
310311
if (_debug_visualizations) {
311312
_expansions_publisher->on_deactivate();
312313
_planned_footprints_publisher->on_deactivate();
313-
_planned_footprints_smoothed_publisher->on_deactivate();
314+
_smoothed_footprints_publisher->on_deactivate();
314315
}
315316
if (_costmap_downsampler) {
316317
_costmap_downsampler->on_deactivate();
@@ -339,7 +340,7 @@ void SmacPlannerHybrid::cleanup()
339340
if (_debug_visualizations) {
340341
_expansions_publisher.reset();
341342
_planned_footprints_publisher.reset();
342-
_planned_footprints_smoothed_publisher.reset();
343+
_smoothed_footprints_publisher.reset();
343344
}
344345
}
345346

@@ -493,9 +494,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
493494

494495
if (_debug_visualizations) {
495496
// Publish expansions for debug
497+
auto now = _clock->now();
496498
geometry_msgs::msg::PoseArray msg;
497499
geometry_msgs::msg::Pose msg_pose;
498-
msg.header.stamp = _clock->now();
500+
msg.header.stamp = now;
499501
msg.header.frame_id = _global_frame;
500502
for (auto & e : *expansions) {
501503
msg_pose.position.x = std::get<0>(e);
@@ -505,19 +507,20 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
505507
}
506508
_expansions_publisher->publish(msg);
507509

508-
// plot footprint path planned for debug
509510
if (_planned_footprints_publisher->get_subscription_count() > 0) {
511+
// Clear all markers first
510512
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
513+
visualization_msgs::msg::Marker clear_all_marker;
514+
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
515+
marker_array->markers.push_back(clear_all_marker);
516+
_planned_footprints_publisher->publish(std::move(marker_array));
517+
518+
// Publish smoothed footprints for debug
519+
marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
511520
for (size_t i = 0; i < plan.poses.size(); i++) {
512521
const std::vector<geometry_msgs::msg::Point> edge =
513522
transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
514-
marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now()));
515-
}
516-
517-
if (marker_array->markers.empty()) {
518-
visualization_msgs::msg::Marker clear_all_marker;
519-
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
520-
marker_array->markers.push_back(clear_all_marker);
523+
marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
521524
}
522525
_planned_footprints_publisher->publish(std::move(marker_array));
523526
}
@@ -546,23 +549,23 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
546549
#endif
547550

548551
if (_debug_visualizations) {
549-
// plot footprint path planned for debug
550-
if (_planned_footprints_smoothed_publisher->get_subscription_count() > 0) {
552+
if (_smoothed_footprints_publisher->get_subscription_count() > 0) {
553+
// Clear all markers first
551554
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
552-
553555
visualization_msgs::msg::Marker clear_all_marker;
554556
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
555557
marker_array->markers.push_back(clear_all_marker);
558+
_smoothed_footprints_publisher->publish(std::move(marker_array));
556559

557-
_planned_footprints_smoothed_publisher->publish(std::move(marker_array));
558-
560+
// Publish smoothed footprints for debug
559561
marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
562+
auto now = _clock->now();
560563
for (size_t i = 0; i < plan.poses.size(); i++) {
561564
const std::vector<geometry_msgs::msg::Point> edge =
562565
transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
563-
marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now()));
566+
marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
564567
}
565-
_planned_footprints_smoothed_publisher->publish(std::move(marker_array));
568+
_smoothed_footprints_publisher->publish(std::move(marker_array));
566569
}
567570
}
568571

‎nav2_smac_planner/src/smac_planner_lattice.cpp‎

Lines changed: 26 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -214,8 +214,9 @@ void SmacPlannerLattice::configure(
214214
_expansions_publisher = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
215215
_planned_footprints_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
216216
"planned_footprints", 1);
217-
_planned_footprints_smoothed_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
218-
"planned_footprints_smoothed", 1);
217+
_smoothed_footprints_publisher =
218+
node->create_publisher<visualization_msgs::msg::MarkerArray>(
219+
"smoothed_footprints", 1);
219220
}
220221

221222
RCLCPP_INFO(
@@ -236,7 +237,7 @@ void SmacPlannerLattice::activate()
236237
if (_debug_visualizations) {
237238
_expansions_publisher->on_activate();
238239
_planned_footprints_publisher->on_activate();
239-
_planned_footprints_smoothed_publisher->on_activate();
240+
_smoothed_footprints_publisher->on_activate();
240241
}
241242
auto node = _node.lock();
242243
// Add callback for dynamic parameters
@@ -253,7 +254,7 @@ void SmacPlannerLattice::deactivate()
253254
if (_debug_visualizations) {
254255
_expansions_publisher->on_deactivate();
255256
_planned_footprints_publisher->on_deactivate();
256-
_planned_footprints_smoothed_publisher->on_deactivate();
257+
_smoothed_footprints_publisher->on_deactivate();
257258
}
258259
// shutdown dyn_param_handler
259260
auto node = _node.lock();
@@ -275,7 +276,7 @@ void SmacPlannerLattice::cleanup()
275276
if (_debug_visualizations) {
276277
_expansions_publisher.reset();
277278
_planned_footprints_publisher.reset();
278-
_planned_footprints_smoothed_publisher.reset();
279+
_smoothed_footprints_publisher.reset();
279280
}
280281
}
281282

@@ -370,9 +371,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
370371
_tolerance / static_cast<float>(_costmap->getResolution()), cancel_checker, expansions.get()))
371372
{
372373
if (_debug_visualizations) {
374+
auto now = _clock->now();
373375
geometry_msgs::msg::PoseArray msg;
374376
geometry_msgs::msg::Pose msg_pose;
375-
msg.header.stamp = _clock->now();
377+
msg.header.stamp = now;
376378
msg.header.frame_id = _global_frame;
377379
for (auto & e : *expansions) {
378380
msg_pose.position.x = std::get<0>(e);
@@ -421,10 +423,11 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
421423
}
422424

423425
if (_debug_visualizations) {
426+
auto now = _clock->now();
424427
// Publish expansions for debug
425428
geometry_msgs::msg::PoseArray msg;
426429
geometry_msgs::msg::Pose msg_pose;
427-
msg.header.stamp = _clock->now();
430+
msg.header.stamp = now;
428431
msg.header.frame_id = _global_frame;
429432
for (auto & e : *expansions) {
430433
msg_pose.position.x = std::get<0>(e);
@@ -434,19 +437,20 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
434437
}
435438
_expansions_publisher->publish(msg);
436439

437-
// plot footprint path planned for debug
438440
if (_planned_footprints_publisher->get_subscription_count() > 0) {
441+
// Clear all markers first
439442
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
443+
visualization_msgs::msg::Marker clear_all_marker;
444+
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
445+
marker_array->markers.push_back(clear_all_marker);
446+
_planned_footprints_publisher->publish(std::move(marker_array));
447+
448+
// Publish smoothed footprints for debug
449+
marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
440450
for (size_t i = 0; i < plan.poses.size(); i++) {
441451
const std::vector<geometry_msgs::msg::Point> edge =
442452
transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
443-
marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now()));
444-
}
445-
446-
if (marker_array->markers.empty()) {
447-
visualization_msgs::msg::Marker clear_all_marker;
448-
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
449-
marker_array->markers.push_back(clear_all_marker);
453+
marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
450454
}
451455
_planned_footprints_publisher->publish(std::move(marker_array));
452456
}
@@ -475,23 +479,23 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
475479
#endif
476480

477481
if (_debug_visualizations) {
478-
// plot footprint path planned for debug
479-
if (_planned_footprints_smoothed_publisher->get_subscription_count() > 0) {
482+
if (_smoothed_footprints_publisher->get_subscription_count() > 0) {
483+
// Clear all markers first
480484
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
481-
482485
visualization_msgs::msg::Marker clear_all_marker;
483486
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
484487
marker_array->markers.push_back(clear_all_marker);
488+
_smoothed_footprints_publisher->publish(std::move(marker_array));
485489

486-
_planned_footprints_smoothed_publisher->publish(std::move(marker_array));
487-
490+
// Publish smoothed footprints for debug
488491
marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
492+
auto now = _clock->now();
489493
for (size_t i = 0; i < plan.poses.size(); i++) {
490494
const std::vector<geometry_msgs::msg::Point> edge =
491495
transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
492-
marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now()));
496+
marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
493497
}
494-
_planned_footprints_smoothed_publisher->publish(std::move(marker_array));
498+
_smoothed_footprints_publisher->publish(std::move(marker_array));
495499
}
496500
}
497501

0 commit comments

Comments
 (0)