Iron sync 2, Aug 4, 1.2.2 (ros-navigation#3740)

* Only apply Wnon-virtual-dtor if the compile language is CXX (ros-navigation#3614)

* https://stackoverflow.com/questions/25525047/cmake-generator-expression-differentiate-c-c-code

Signed-off-by: Ryan Friedman <[email protected]>

* Fix map not showing on rviz when navigation is launched with namespace (ros-navigation#3620)

* Fix Wshadow errors and enforce it (ros-navigation#3617)

* Fix Wshadow errors and enforce it

Signed-off-by: Ryan Friedman <[email protected]>

* Remove workaround for pluginlib

* This was only needed because it was included transitively
* By finding and linking properly, the compiler flags get propogated as SYSTEM
  correctly

Signed-off-by: Ryan Friedman <[email protected]>

---------

Signed-off-by: Ryan Friedman <[email protected]>

* add-Wnull-dereference and fix warnings (ros-navigation#3622)

Signed-off-by: Ryan Friedman <[email protected]>

* updating mppi's path angle critic for optional bidirectionality (ros-navigation#3624)

* updating mppi's path angle critic for optional bidirectionality

* Update README.md

* correct error message (ros-navigation#3631)

* correct error message

* clean up

* cleanup

* remove header

* Let Navigators have different error codes (ros-navigation#3642)

* Change ERROR to DEBUG

* INFO message on init

* format code

* Replace newlines with spaces

* fixing path angle critic's non-directional bias (ros-navigation#3632)

* fixing path angle critic's non-directional bias

* adding reformat

* adapting goal critic for speed to goal (ros-navigation#3641)

* adapting goal critic for speed to goal

* retuning goal critic

* add readme entries

* Update critics_tests.cpp

* Fix uninitialized value (ros-navigation#3651)

* In NAV2, this warning is treated as an error

Signed-off-by: Ryan Friedman <[email protected]>

* Fix rviz panel node arguments (ros-navigation#3655)

Signed-off-by: Nick Lamprianidis <[email protected]>

* Reduce out-of-range log to DEBUG (ros-navigation#3656)

* Adding nan twist rejection for velocity smoother and collision monitor (ros-navigation#3658)

* adding nan twist rejection for velocity smoother and collision monitor

* deref

* Ceres exposes a namespaced export and recommends it in their docs (ros-navigation#3652)

Signed-off-by: Ryan Friedman <[email protected]>

* Enable multiple MPPI path angle modes depending on preferences in behavior (ros-navigation#3650)

* fixing path angle critic's non-directional bias

* adding reformat

* handle linting

* add utility unit tests

* adding unit tests for path angle

* MPPI: Support Exact Path Following For Feasible Plans (ros-navigation#3659)

* alternative to path align critic for inversion control

* fix default behavior (enforce_path_inversion: false) (ros-navigation#3643)

Co-authored-by: Guillaume Doisy <[email protected]>

* adding dyaw option for path alignment to incentivize following the path's intent where necessary

* add docs for use path orientations

* fix typo

---------

Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>

* Resolves 3646: Update CMAKE_COMPILER_IS_GNUCXX (ros-navigation#3662)

* Resolves 3646: Update CMAKE_COMPILER_IS_GNUCXX

* Update CMakeLists.txt

* Fix smoother server tests (ros-navigation#3663)

* Fix smoother server tests

* Update test_smoother_server.cpp

* fix some new build warnings from sync (ros-navigation#3674)

* fix some new build warnings

* fixing last issue

* Update navigate_through_poses_action.hpp

* adding unsigned int to tests

* all to unsigned shorts

* test new warning resolution

* Update

* convert unsigned shorts to uint16_t for linter

* Fix costmap publisher test (ros-navigation#3679)

* added printouts

* ignore system tests

* fix

* cleanup

* Update test_costmap_2d_publisher.cpp

remove space

* remove empty message (ros-navigation#3691)

* Collision Monitor fixups (ros-navigation#3696)

* Fix max_points -> min_points in parameters
* Move robot_utils.hpp include to source where it actually using
* Remove double-description of getTransform()

* Use ParameterFile (allow_substs) (ros-navigation#3706)

Signed-off-by: ymd-stella <[email protected]>

* nav2_bt_navigator: log current location on navigate_to_pose action initialization (ros-navigation#3720)

It is very useful to know the current location considered by the
bt_navigator for debug purposes.

* nav2_behaviors: export all available plugins (ros-navigation#3716)

It allows external packages to include those headers and create child
classes through inheritance.

* changing costmap layers private to protected (ros-navigation#3722)

* Update costmap_2d_ros.cpp (ros-navigation#3687)

* updated nav2_behavior_tree test util install path (ros-navigation#3718)

* launch linting (ros-navigation#3729)

* adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially (ros-navigation#3728)

* adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially

* fix test failures

* Update RewrittenYaml to support list rewrites (ros-navigation#3727)

* allowing leaf key rewrites that aren't dcits (ros-navigation#3730)

* adding checks on config and dynamic parameters for proper velocity and acceleration limits (ros-navigation#3731)

* Fix Goal updater QoS (ros-navigation#3719)

* Fix GoalUpdater QoS

* Fixes

* adding tolerance back in for smac lattice and hybrid-A* planners (ros-navigation#3734)

* Completing Hybrid-A* visualization of expansion footprints PR (ros-navigation#3733)

* smach_planner_hybrid: add support visualization for hybrid Astar

* smac_planner_hyrid: revert some

* smach_planner_hybrid: improving code quality

* utils: add some useful functions

* utils: fix mistake

* nav2_smac_planner: fix format problem

* utils: fix format and revise functions

* smach_planner_hybrid: delete _viz_expansion parameter

* smac_planner_hybrid: fix format

* README: update parameter

* utils: corrct mistake return

* utils: make timestamp a const reference

* nav2_smac_planner: correct format problem

* add unit test functions

* further detection of element equality

* test_utils: add non-trival translation and rotation

* smac_planner_hybrid: pass value instead of references

* completing hybrid A* visualization

---------

Co-authored-by: xianglunkai <[email protected]>

* Update README.md (ros-navigation#3736)

* Update README.md

* Update README.md

* sync iron to 1.2.2 to release

---------

Signed-off-by: Ryan Friedman <[email protected]>
Signed-off-by: Nick Lamprianidis <[email protected]>
Signed-off-by: ymd-stella <[email protected]>
Co-authored-by: Ryan <[email protected]>
Co-authored-by: Filipe Cerveira <[email protected]>
Co-authored-by: Joshua Wallace <[email protected]>
Co-authored-by: RBT22 <[email protected]>
Co-authored-by: Nick Lamprianidis <[email protected]>
Co-authored-by: BriceRenaudeau <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: ymd-stella <[email protected]>
Co-authored-by: DylanDeCoeyer-Quimesis <[email protected]>
Co-authored-by: Aaditya Ravindran <[email protected]>
Co-authored-by: gyaanantia <[email protected]>
Co-authored-by: Tony Najjar <[email protected]>
Co-authored-by: xianglunkai <[email protected]>
Signed-off-by: MarcM0 <[email protected]>

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -253,8 +253,11 @@ void SmacPlannerHybrid::configure(
   }
 
   _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
-  if (_viz_expansions) {
+
+  if (_debug_visualizations) {
     _expansions_publisher = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
+    _planned_footprints_publisher = node->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "planned_footprints", 1);
   }
 
   if (_debug_visualizations) {
@@ -457,20 +460,6 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
     }
   }
 
-  // Publish expansions for debug
-  if (_viz_expansions) {
-    geometry_msgs::msg::PoseArray msg;
-    geometry_msgs::msg::Pose msg_pose;
-    msg.header.stamp = _clock->now();
-    msg.header.frame_id = _global_frame;
-    for (auto & e : *expansions) {
-      msg_pose.position.x = std::get<0>(e);
-      msg_pose.position.y = std::get<1>(e);
-      msg.poses.push_back(msg_pose);
-    }
-    _expansions_publisher->publish(msg);
-  }
-
   // Convert to world coordinates
   plan.poses.reserve(path.size());
   for (int i = path.size() - 1; i >= 0; --i) {