Skip to content

Commit 36ca71d

Browse files
tonynajjartonynajjar
authored
Publish planned footprints after smoothing (#5155)
* Publish planned footprints after smoothing Signed-off-by: Tony Najjar <[email protected]> * Revert "Publish planned footprints after smoothing" This reverts commit c9b349a. * Add smoothed footprints publishing Signed-off-by: Tony Najjar <[email protected]> * fix formatting Signed-off-by: Tony Najjar <[email protected]> * Fix indentation for publisher creation in SmacPlannerHybrid and SmacPlannerLattice Signed-off-by: Tony Najjar <[email protected]> * address PR comments Signed-off-by: Tony Najjar <[email protected]> * fixes Signed-off-by: Tony Najjar <[email protected]> * fix build error Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]>
1 parent cd7a0b7 commit 36ca71d

File tree

4 files changed

+92
-22
lines changed

4 files changed

+92
-22
lines changed

‎nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -128,6 +128,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
128128
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
129129
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
130130
_planned_footprints_publisher;
131+
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
132+
_smoothed_footprints_publisher;
131133
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
132134
_expansions_publisher;
133135
std::mutex _mutex;

‎nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -118,6 +118,8 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
118118
bool _debug_visualizations;
119119
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
120120
_planned_footprints_publisher;
121+
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
122+
_smoothed_footprints_publisher;
121123
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
122124
_expansions_publisher;
123125
GoalHeadingMode _goal_heading_mode;

‎nav2_smac_planner/src/smac_planner_hybrid.cpp‎

Lines changed: 42 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -294,6 +294,9 @@ void SmacPlannerHybrid::configure(
294294
_expansions_publisher = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
295295
_planned_footprints_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
296296
"planned_footprints", 1);
297+
_smoothed_footprints_publisher =
298+
node->create_publisher<visualization_msgs::msg::MarkerArray>(
299+
"smoothed_footprints", 1);
297300
}
298301

299302
RCLCPP_INFO(
@@ -314,6 +317,7 @@ void SmacPlannerHybrid::activate()
314317
if (_debug_visualizations) {
315318
_expansions_publisher->on_activate();
316319
_planned_footprints_publisher->on_activate();
320+
_smoothed_footprints_publisher->on_activate();
317321
}
318322
if (_costmap_downsampler) {
319323
_costmap_downsampler->on_activate();
@@ -342,6 +346,7 @@ void SmacPlannerHybrid::deactivate()
342346
if (_debug_visualizations) {
343347
_expansions_publisher->on_deactivate();
344348
_planned_footprints_publisher->on_deactivate();
349+
_smoothed_footprints_publisher->on_deactivate();
345350
}
346351
if (_costmap_downsampler) {
347352
_costmap_downsampler->on_deactivate();
@@ -367,8 +372,11 @@ void SmacPlannerHybrid::cleanup()
367372
_costmap_downsampler.reset();
368373
}
369374
_raw_plan_publisher.reset();
370-
_expansions_publisher.reset();
371-
_planned_footprints_publisher.reset();
375+
if (_debug_visualizations) {
376+
_expansions_publisher.reset();
377+
_planned_footprints_publisher.reset();
378+
_smoothed_footprints_publisher.reset();
379+
}
372380
}
373381

374382
nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
@@ -522,9 +530,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
522530

523531
if (_debug_visualizations) {
524532
// Publish expansions for debug
533+
auto now = _clock->now();
525534
geometry_msgs::msg::PoseArray msg;
526535
geometry_msgs::msg::Pose msg_pose;
527-
msg.header.stamp = _clock->now();
536+
msg.header.stamp = now;
528537
msg.header.frame_id = _global_frame;
529538
for (auto & e : *expansions) {
530539
msg_pose.position.x = std::get<0>(e);
@@ -534,19 +543,20 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
534543
}
535544
_expansions_publisher->publish(msg);
536545

537-
// plot footprint path planned for debug
538546
if (_planned_footprints_publisher->get_subscription_count() > 0) {
547+
// Clear all markers first
539548
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
549+
visualization_msgs::msg::Marker clear_all_marker;
550+
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
551+
marker_array->markers.push_back(clear_all_marker);
552+
_planned_footprints_publisher->publish(std::move(marker_array));
553+
554+
// Publish smoothed footprints for debug
555+
marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
540556
for (size_t i = 0; i < plan.poses.size(); i++) {
541557
const std::vector<geometry_msgs::msg::Point> edge =
542558
transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
543-
marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now()));
544-
}
545-
546-
if (marker_array->markers.empty()) {
547-
visualization_msgs::msg::Marker clear_all_marker;
548-
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
549-
marker_array->markers.push_back(clear_all_marker);
559+
marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
550560
}
551561
_planned_footprints_publisher->publish(std::move(marker_array));
552562
}
@@ -574,6 +584,27 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
574584
" milliseconds to smooth path." << std::endl;
575585
#endif
576586

587+
if (_debug_visualizations) {
588+
if (_smoothed_footprints_publisher->get_subscription_count() > 0) {
589+
// Clear all markers first
590+
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
591+
visualization_msgs::msg::Marker clear_all_marker;
592+
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
593+
marker_array->markers.push_back(clear_all_marker);
594+
_smoothed_footprints_publisher->publish(std::move(marker_array));
595+
596+
// Publish smoothed footprints for debug
597+
marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
598+
auto now = _clock->now();
599+
for (size_t i = 0; i < plan.poses.size(); i++) {
600+
const std::vector<geometry_msgs::msg::Point> edge =
601+
transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
602+
marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
603+
}
604+
_smoothed_footprints_publisher->publish(std::move(marker_array));
605+
}
606+
}
607+
577608
return plan;
578609
}
579610

‎nav2_smac_planner/src/smac_planner_lattice.cpp‎

Lines changed: 46 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,6 @@ void SmacPlannerLattice::configure(
5656
_costmap_ros = costmap_ros;
5757
_name = name;
5858
_global_frame = costmap_ros->getGlobalFrameID();
59-
_raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
6059

6160
RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerLattice", name.c_str());
6261

@@ -238,10 +237,15 @@ void SmacPlannerLattice::configure(
238237
_smoother->initialize(_metadata.min_turning_radius);
239238
}
240239

240+
_raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
241+
241242
if (_debug_visualizations) {
242243
_expansions_publisher = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
243244
_planned_footprints_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
244245
"planned_footprints", 1);
246+
_smoothed_footprints_publisher =
247+
node->create_publisher<visualization_msgs::msg::MarkerArray>(
248+
"smoothed_footprints", 1);
245249
}
246250

247251
RCLCPP_INFO(
@@ -262,6 +266,7 @@ void SmacPlannerLattice::activate()
262266
if (_debug_visualizations) {
263267
_expansions_publisher->on_activate();
264268
_planned_footprints_publisher->on_activate();
269+
_smoothed_footprints_publisher->on_activate();
265270
}
266271
auto node = _node.lock();
267272
// Add callback for dynamic parameters
@@ -278,6 +283,7 @@ void SmacPlannerLattice::deactivate()
278283
if (_debug_visualizations) {
279284
_expansions_publisher->on_deactivate();
280285
_planned_footprints_publisher->on_deactivate();
286+
_smoothed_footprints_publisher->on_deactivate();
281287
}
282288
// shutdown dyn_param_handler
283289
auto node = _node.lock();
@@ -296,6 +302,11 @@ void SmacPlannerLattice::cleanup()
296302
_a_star.reset();
297303
_smoother.reset();
298304
_raw_plan_publisher.reset();
305+
if (_debug_visualizations) {
306+
_expansions_publisher.reset();
307+
_planned_footprints_publisher.reset();
308+
_smoothed_footprints_publisher.reset();
309+
}
299310
}
300311

301312
nav_msgs::msg::Path SmacPlannerLattice::createPlan(
@@ -390,9 +401,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
390401
_tolerance / static_cast<float>(_costmap->getResolution()), cancel_checker, expansions.get()))
391402
{
392403
if (_debug_visualizations) {
404+
auto now = _clock->now();
393405
geometry_msgs::msg::PoseArray msg;
394406
geometry_msgs::msg::Pose msg_pose;
395-
msg.header.stamp = _clock->now();
407+
msg.header.stamp = now;
396408
msg.header.frame_id = _global_frame;
397409
for (auto & e : *expansions) {
398410
msg_pose.position.x = std::get<0>(e);
@@ -441,10 +453,11 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
441453
}
442454

443455
if (_debug_visualizations) {
456+
auto now = _clock->now();
444457
// Publish expansions for debug
445458
geometry_msgs::msg::PoseArray msg;
446459
geometry_msgs::msg::Pose msg_pose;
447-
msg.header.stamp = _clock->now();
460+
msg.header.stamp = now;
448461
msg.header.frame_id = _global_frame;
449462
for (auto & e : *expansions) {
450463
msg_pose.position.x = std::get<0>(e);
@@ -454,19 +467,20 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
454467
}
455468
_expansions_publisher->publish(msg);
456469

457-
// plot footprint path planned for debug
458470
if (_planned_footprints_publisher->get_subscription_count() > 0) {
471+
// Clear all markers first
459472
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
473+
visualization_msgs::msg::Marker clear_all_marker;
474+
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
475+
marker_array->markers.push_back(clear_all_marker);
476+
_planned_footprints_publisher->publish(std::move(marker_array));
477+
478+
// Publish smoothed footprints for debug
479+
marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
460480
for (size_t i = 0; i < plan.poses.size(); i++) {
461481
const std::vector<geometry_msgs::msg::Point> edge =
462482
transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
463-
marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now()));
464-
}
465-
466-
if (marker_array->markers.empty()) {
467-
visualization_msgs::msg::Marker clear_all_marker;
468-
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
469-
marker_array->markers.push_back(clear_all_marker);
483+
marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
470484
}
471485
_planned_footprints_publisher->publish(std::move(marker_array));
472486
}
@@ -494,6 +508,27 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
494508
" milliseconds to smooth path." << std::endl;
495509
#endif
496510

511+
if (_debug_visualizations) {
512+
if (_smoothed_footprints_publisher->get_subscription_count() > 0) {
513+
// Clear all markers first
514+
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
515+
visualization_msgs::msg::Marker clear_all_marker;
516+
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
517+
marker_array->markers.push_back(clear_all_marker);
518+
_smoothed_footprints_publisher->publish(std::move(marker_array));
519+
520+
// Publish smoothed footprints for debug
521+
marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
522+
auto now = _clock->now();
523+
for (size_t i = 0; i < plan.poses.size(); i++) {
524+
const std::vector<geometry_msgs::msg::Point> edge =
525+
transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint());
526+
marker_array->markers.push_back(createMarker(edge, i, _global_frame, now));
527+
}
528+
_smoothed_footprints_publisher->publish(std::move(marker_array));
529+
}
530+
}
531+
497532
return plan;
498533
}
499534

0 commit comments

Comments
 (0)