Ceres smoother (ros-navigation#2753)

* old version of smoother server with path truncation and without collision checker and footprint subscriber

* Removed path truncation from smoother server, added footprint subscriber
and path collision checking

* smoother code styling fixes

* Update nav2_tree_nodes.xml

* added TruncatePathLocal BT action

* truncate_path_local_action bt plugin code styling

* WIP: PR ros-navigation#2569 fixes

* smoother server changes as requested in PR ros-navigation#2569, tests added

* cpplint fixes

* added TruncatePathLocal into bt configs

* added nav2_smoother to composed bringup

* smoother server: improved test coverage, details fixed

* removed smoother from composed bringup lifecycle nodes as it fails on
non-existing plugins

* removed smoother server default plugin since it's not merged yet, readded into bringup

* nav2_smoother tests: added mocked class loader to improve
nav2_smoother.cpp test coverage

* nav2_smoother 100% test coverage

* test_smoother_server cpplint fix

* test_smoother_server added cleanup transition

* nav2_smoother added default plugin type autoconfig test

* added nav2_ceres_costaware_smoother

* changed rtv_ to nav2_

* Update nav2_smoother/README.md

* nav2_ceres_costaware_smoother cleanup

* removed smoother_server_rclcpp_node from nav2_params.yaml, added
smoother_server to other config YAML files

* registered nav2_smoother component

* registered nav2_smoother component

* fixed header uniquity macros

* fixed nav2_smoother_core to be shared

* fixed nav2_smoother build

* minor cleanup, test interface added

* added smoothness, distance and curvature tests

* added more tests

* ceres smoother cleanup, minor optimizations

* ceres smoother: fixed interpolator cleanup bug, added tests, more code cleanup

* ceres smoother fixes and tests

* truncate_path_local bt node: changed API to be more similar to other
nav2 bt nodes, tests written
nav2_ceres_costaware_smoother: changed ASSERT to EXPECT in tests

* ceres smoother uncrustified, fixed linting errors

* updated readme

* ceres smoother: updated readme

* fixed signed-unsigned comparison error

* removing obsolete max_time parameter since max_time is specified in
smooth action goal

* ceres smoother: updated readme

* Update options.hpp

* by-passing path orientations approximation when skipping smac smoother

* moved smac smoother by-pass directly to smooth method

* fixed smac smoother tests

* removed debug logger

* Removed merge bug from nav2_smoother/package.xml

* ceres smoother: added keep start/goal orientation parameters

* ceres smoother: added tests for start/goal orientations

* ceres smoother: resolved requested details (name will be changed in
following commit to prevent git clutter)

* unified point variable names, added tests for edge-cases

* ceres smoother: added comments, made parameters more intuitive, improved
tests

* ceres smoother: adding coments and cost check point weights
normalization

* ceres smoother: renamed to constrained smoother

* TruncatePathLocal BT node: cleaned up, added max_robot_pose_search_dist

* TruncatePathLocal: improved readability, added comments

* constrained smoother: added readme note about not supporting cusp
detection without pose orientations assigned

* constrained smoother: updated readme

* Constrained smoother: fixed readme and comments

* Constrained smoother ReadMe: added recommended usage note

Co-authored-by: afrixs <matej.vargovcik@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Matej Vargovcik <vargovcik@robotechvision.com>

Author: 4 people

nav2_bringup/params/nav2_multirobot_params_1.yaml

@@ -80,6 +80,7 @@ bt_navigator:
     - nav2_distance_controller_bt_node
     - nav2_speed_controller_bt_node
     - nav2_truncate_path_action_bt_node
+    - nav2_truncate_path_local_action_bt_node
     - nav2_goal_updater_node_bt_node
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node

nav2_bringup/params/nav2_multirobot_params_2.yaml

@@ -80,6 +80,7 @@ bt_navigator:
     - nav2_distance_controller_bt_node
     - nav2_speed_controller_bt_node
     - nav2_truncate_path_action_bt_node
+    - nav2_truncate_path_local_action_bt_node
     - nav2_goal_updater_node_bt_node
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node

nav2_bringup/params/nav2_params.yaml

@@ -80,6 +80,7 @@ bt_navigator:
     - nav2_distance_controller_bt_node
     - nav2_speed_controller_bt_node
     - nav2_truncate_path_action_bt_node
+    - nav2_truncate_path_local_action_bt_node
     - nav2_goal_updater_node_bt_node
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node