From 3a1d41e553efbea1d76aa885acf1be5368cd77f7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 4 Aug 2023 13:02:15 -0700 Subject: [PATCH] Iron sync 2, Aug 4, 1.2.2 (#3740) * Only apply Wnon-virtual-dtor if the compile language is CXX (#3614) * https://stackoverflow.com/questions/25525047/cmake-generator-expression-differentiate-c-c-code Signed-off-by: Ryan Friedman * Fix map not showing on rviz when navigation is launched with namespace (#3620) * Fix Wshadow errors and enforce it (#3617) * Fix Wshadow errors and enforce it Signed-off-by: Ryan Friedman * 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 --------- Signed-off-by: Ryan Friedman * add-Wnull-dereference and fix warnings (#3622) Signed-off-by: Ryan Friedman * updating mppi's path angle critic for optional bidirectionality (#3624) * updating mppi's path angle critic for optional bidirectionality * Update README.md * correct error message (#3631) * correct error message * clean up * cleanup * remove header * Let Navigators have different error codes (#3642) * Change ERROR to DEBUG * INFO message on init * format code * Replace newlines with spaces * fixing path angle critic's non-directional bias (#3632) * fixing path angle critic's non-directional bias * adding reformat * adapting goal critic for speed to goal (#3641) * adapting goal critic for speed to goal * retuning goal critic * add readme entries * Update critics_tests.cpp * Fix uninitialized value (#3651) * In NAV2, this warning is treated as an error Signed-off-by: Ryan Friedman * Fix rviz panel node arguments (#3655) Signed-off-by: Nick Lamprianidis * Reduce out-of-range log to DEBUG (#3656) * Adding nan twist rejection for velocity smoother and collision monitor (#3658) * adding nan twist rejection for velocity smoother and collision monitor * deref * Ceres exposes a namespaced export and recommends it in their docs (#3652) Signed-off-by: Ryan Friedman * Enable multiple MPPI path angle modes depending on preferences in behavior (#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 (#3659) * alternative to path align critic for inversion control * fix default behavior (enforce_path_inversion: false) (#3643) Co-authored-by: Guillaume Doisy * 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 Co-authored-by: Guillaume Doisy * Resolves 3646: Update CMAKE_COMPILER_IS_GNUCXX (#3662) * Resolves 3646: Update CMAKE_COMPILER_IS_GNUCXX * Update CMakeLists.txt * Fix smoother server tests (#3663) * Fix smoother server tests * Update test_smoother_server.cpp * fix some new build warnings from sync (#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 (#3679) * added printouts * ignore system tests * fix * cleanup * Update test_costmap_2d_publisher.cpp remove space * remove empty message (#3691) * Collision Monitor fixups (#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) (#3706) Signed-off-by: ymd-stella * nav2_bt_navigator: log current location on navigate_to_pose action initialization (#3720) It is very useful to know the current location considered by the bt_navigator for debug purposes. * nav2_behaviors: export all available plugins (#3716) It allows external packages to include those headers and create child classes through inheritance. * changing costmap layers private to protected (#3722) * Update costmap_2d_ros.cpp (#3687) * updated nav2_behavior_tree test util install path (#3718) * launch linting (#3729) * adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially (#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 (#3727) * allowing leaf key rewrites that aren't dcits (#3730) * adding checks on config and dynamic parameters for proper velocity and acceleration limits (#3731) * Fix Goal updater QoS (#3719) * Fix GoalUpdater QoS * Fixes * adding tolerance back in for smac lattice and hybrid-A* planners (#3734) * Completing Hybrid-A* visualization of expansion footprints PR (#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 <1322099375@qq.com> * Update README.md (#3736) * Update README.md * Update README.md * sync iron to 1.2.2 to release --------- Signed-off-by: Ryan Friedman Signed-off-by: Nick Lamprianidis Signed-off-by: ymd-stella Co-authored-by: Ryan Co-authored-by: Filipe Cerveira Co-authored-by: Joshua Wallace Co-authored-by: RBT22 <43723477+RBT22@users.noreply.github.com> Co-authored-by: Nick Lamprianidis Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: ymd-stella <7959916+ymd-stella@users.noreply.github.com> Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: Aaditya Ravindran Co-authored-by: gyaanantia <78275262+gyaanantia@users.noreply.github.com> Co-authored-by: Tony Najjar Co-authored-by: xianglunkai <1322099375@qq.com> --- README.md | 13 ++ nav2_amcl/package.xml | 2 +- nav2_amcl/src/pf/eig3.c | 4 +- nav2_behavior_tree/CMakeLists.txt | 2 +- .../bt_action_server_impl.hpp | 33 ++-- .../action/navigate_through_poses_action.hpp | 4 +- .../are_error_codes_present_condition.hpp | 12 +- nav2_behavior_tree/package.xml | 2 +- .../plugins/decorator/goal_updater_node.cpp | 15 +- .../test_are_error_codes_present.cpp | 6 +- .../test_would_a_controller_recovery_help.cpp | 4 +- .../test_would_a_planner_recovery_help.cpp | 4 +- .../test_would_a_smoother_recovery_help.cpp | 4 +- .../decorator/test_goal_updater_node.cpp | 89 +++++++++-- .../plugins/assisted_teleop.hpp | 0 .../nav2_behaviors}/plugins/back_up.hpp | 0 .../plugins/drive_on_heading.hpp | 0 .../nav2_behaviors}/plugins/spin.hpp | 0 .../nav2_behaviors}/plugins/wait.hpp | 0 nav2_behaviors/package.xml | 2 +- nav2_behaviors/plugins/assisted_teleop.cpp | 2 +- nav2_behaviors/plugins/back_up.cpp | 2 +- nav2_behaviors/plugins/drive_on_heading.cpp | 2 +- nav2_behaviors/plugins/spin.cpp | 2 +- nav2_behaviors/plugins/wait.cpp | 2 +- nav2_bringup/launch/bringup_launch.py | 13 +- nav2_bringup/launch/localization_launch.py | 16 +- nav2_bringup/launch/navigation_launch.py | 14 +- nav2_bringup/launch/rviz_launch.py | 3 +- nav2_bringup/launch/slam_launch.py | 13 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- .../src/navigators/navigate_to_pose.cpp | 9 +- .../launch/collision_monitor_node.launch.py | 14 +- nav2_collision_monitor/package.xml | 2 +- .../params/collision_monitor_params.yaml | 2 +- .../src/collision_monitor_node.cpp | 7 + nav2_collision_monitor/src/range.cpp | 2 +- nav2_common/cmake/nav2_package.cmake | 5 +- .../nav2_common/launch/rewritten_yaml.py | 12 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/CMakeLists.txt | 3 +- .../nav2_constrained_smoother/smoother.hpp | 4 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- .../denoise/image_processing.hpp | 5 +- .../nav2_costmap_2d/inflation_layer.hpp | 1 - .../nav2_costmap_2d/range_sensor_layer.hpp | 2 +- .../include/nav2_costmap_2d/static_layer.hpp | 2 +- .../include/nav2_costmap_2d/voxel_layer.hpp | 1 - nav2_costmap_2d/package.xml | 2 +- .../costmap_filters/costmap_filter.cpp | 2 +- nav2_costmap_2d/plugins/denoise_layer.cpp | 4 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 4 +- .../integration/test_costmap_2d_publisher.cpp | 13 +- .../test_costmap_topic_collision_checker.cpp | 4 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- .../dwb_core/src/dwb_local_planner.cpp | 4 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- .../test/obstacle_footprint_test.cpp | 3 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_mppi_controller/README.md | 35 ++-- .../critics/goal_critic.hpp | 1 + .../critics/path_align_critic.hpp | 1 + .../critics/path_angle_critic.hpp | 30 ++++ .../critics/path_follow_critic.hpp | 1 + .../tools/path_handler.hpp | 19 ++- .../nav2_mppi_controller/tools/utils.hpp | 91 ++++++++++- nav2_mppi_controller/package.xml | 2 +- .../src/critics/constraint_critic.cpp | 5 +- .../src/critics/goal_angle_critic.cpp | 2 +- .../src/critics/goal_critic.cpp | 15 +- .../src/critics/obstacles_critic.cpp | 13 +- .../src/critics/path_align_critic.cpp | 26 ++- .../src/critics/path_angle_critic.cpp | 94 +++++++++-- .../src/critics/path_follow_critic.cpp | 2 +- .../src/critics/prefer_forward_critic.cpp | 2 +- nav2_mppi_controller/src/path_handler.cpp | 52 +++++- .../test/critic_manager_test.cpp | 14 +- nav2_mppi_controller/test/critics_tests.cpp | 79 +++++++++- .../test/path_handler_test.cpp | 72 ++++++++- nav2_mppi_controller/test/utils_test.cpp | 85 +++++++++- nav2_msgs/action/NavigateThroughPoses.action | 1 - nav2_msgs/action/NavigateToPose.action | 1 - nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_navfn_planner/src/navfn.cpp | 58 ++++--- nav2_planner/package.xml | 2 +- .../package.xml | 2 +- .../src/path_handler.cpp | 4 +- nav2_rotation_shim_controller/package.xml | 2 +- nav2_rviz_plugins/CMakeLists.txt | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_rviz_plugins/src/nav2_panel.cpp | 2 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/README.md | 2 +- .../nav2_smac_planner/collision_checker.hpp | 7 +- .../nav2_smac_planner/smac_planner_hybrid.hpp | 4 +- .../include/nav2_smac_planner/utils.hpp | 76 +++++++++ nav2_smac_planner/package.xml | 2 +- nav2_smac_planner/src/collision_checker.cpp | 20 ++- nav2_smac_planner/src/smac_planner_2d.cpp | 4 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 77 ++++++--- .../src/smac_planner_lattice.cpp | 8 +- nav2_smac_planner/test/CMakeLists.txt | 11 ++ nav2_smac_planner/test/test_a_star.cpp | 14 +- .../test/test_collision_checker.cpp | 24 ++- nav2_smac_planner/test/test_node2d.cpp | 6 +- nav2_smac_planner/test/test_nodehybrid.cpp | 9 +- nav2_smac_planner/test/test_nodelattice.cpp | 18 ++- nav2_smac_planner/test/test_smoother.cpp | 2 +- nav2_smac_planner/test/test_utils.cpp | 149 ++++++++++++++++++ nav2_smoother/package.xml | 2 +- nav2_smoother/test/test_smoother_server.cpp | 42 +++-- nav2_system_tests/CMakeLists.txt | 2 + nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/include/nav2_util/robot_utils.hpp | 20 +-- nav2_util/package.xml | 2 +- nav2_util/src/robot_utils.cpp | 30 ++++ nav2_util/test/test_robot_utils.cpp | 26 +++ .../velocity_smoother.hpp | 1 + nav2_velocity_smoother/package.xml | 2 +- .../src/velocity_smoother.cpp | 37 ++++- .../test/test_velocity_smoother.cpp | 34 ++++ nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 136 files changed, 1409 insertions(+), 359 deletions(-) rename nav2_behaviors/{ => include/nav2_behaviors}/plugins/assisted_teleop.hpp (100%) rename nav2_behaviors/{ => include/nav2_behaviors}/plugins/back_up.hpp (100%) rename nav2_behaviors/{ => include/nav2_behaviors}/plugins/drive_on_heading.hpp (100%) rename nav2_behaviors/{ => include/nav2_behaviors}/plugins/spin.hpp (100%) rename nav2_behaviors/{ => include/nav2_behaviors}/plugins/wait.hpp (100%) create mode 100644 nav2_smac_planner/test/test_utils.cpp diff --git a/README.md b/README.md index 0ec74a7fb7..b8d9131e16 100644 --- a/README.md +++ b/README.md @@ -55,6 +55,19 @@ please cite this work in your papers! } ``` +If you use **any** of the algorithms in Nav2 or the analysis of the algorithms in your work, please cite this work in your papers! + +- S. Macenski, T. Moore, DV Lu, A. Merzlyakov, M. Ferguson, [**From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2**](https://arxiv.org/pdf/2307.15236.pdf), Robotics and Autonomous Systems, 2023. + +```bibtex + @article{macenski2023survey, + title={From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2}, + author={S. Macenski, T. Moore, DV Lu, A. Merzlyakov, M. Ferguson}, + year={2023}, + journal = {Robotics and Autonomous Systems} + } +``` + If you use the Regulated Pure Pursuit Controller algorithm or software from this repository, please cite this work in your papers! - S. Macenski, S. Singh, F. Martin, J. Gines, [**Regulated Pure Pursuit for Robot Path Tracking**](https://arxiv.org/abs/2305.20026). Autonomous Robots, 2023. diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index be04aef4e2..5a395091f0 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.2.1 + 1.2.2

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/pf/eig3.c b/nav2_amcl/src/pf/eig3.c index a5a84ae5df..1980475aa0 100644 --- a/nav2_amcl/src/pf/eig3.c +++ b/nav2_amcl/src/pf/eig3.c @@ -43,7 +43,7 @@ static void tred2(double V[n][n], double d[n], double e[n]) // Fortran subroutine in EISPACK. int i, j, k; - double f, g, h, hh; + double f, g, hh; for (j = 0; j < n; j++) { d[j] = V[n - 1][j]; } @@ -122,7 +122,7 @@ static void tred2(double V[n][n], double d[n], double e[n]) for (i = 0; i < n - 1; i++) { V[n - 1][i] = V[i][i]; V[i][i] = 1.0; - h = d[i + 1]; + const double h = d[i + 1]; if (h != 0.0) { for (k = 0; k <= i; k++) { d[k] = V[k][i + 1] / h; diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index f82e70dc8b..96fc154013 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -224,7 +224,7 @@ install(DIRECTORY include/ ) install(DIRECTORY test/utils/ - DESTINATION include/utils/ + DESTINATION include/${PROJECT_NAME}/utils/ ) install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 11110b09c2..24126ed4ab 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -67,16 +67,29 @@ BtActionServer::BtActionServer( }; if (!node->has_parameter("error_code_names")) { - std::string error_codes_str; - for (const auto & error_code : error_code_names) { - error_codes_str += error_code + "\n"; + const rclcpp::ParameterValue value = node->declare_parameter( + "error_code_names", + rclcpp::PARAMETER_STRING_ARRAY); + if (value.get_type() == rclcpp::PARAMETER_NOT_SET) { + std::string error_codes_str; + for (const auto & error_code : error_code_names) { + error_codes_str += " " + error_code; + } + RCLCPP_WARN_STREAM( + logger_, "Error_code parameters were not set. Using default values of:" + << error_codes_str + "\n" + << "Make sure these match your BT and there are not other sources of error codes you" + "reported to your application"); + rclcpp::Parameter error_code_names_param("error_code_names", error_code_names); + node->set_parameter(error_code_names_param); + } else { + error_code_names = value.get>(); + std::string error_codes_str; + for (const auto & error_code : error_code_names) { + error_codes_str += " " + error_code; + } + RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" << error_codes_str); } - RCLCPP_WARN_STREAM( - logger_, "Error_code parameters were not set. Using default values of: " - << error_codes_str - << "Make sure these match your BT and there are not other sources of error codes you want " - "reported to your application"); - node->declare_parameter("error_code_names", error_code_names); } } @@ -279,7 +292,7 @@ void BtActionServer::populateErrorCode( highest_priority_error_code = current_error_code; } } catch (...) { - RCLCPP_ERROR( + RCLCPP_DEBUG( logger_, "Failed to get error code: %s from blackboard", error_code.c_str()); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 45d0adff10..4b9ac96f7a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -16,6 +16,7 @@ #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_ #include +#include #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -74,7 +75,8 @@ class NavigateThroughPosesAction : public BtActionNode("goals", "Destinations to plan through"), + BT::InputPort>( + "goals", "Destinations to plan through"), BT::InputPort("behavior_tree", "Behavior tree to run"), BT::OutputPort( "error_code_id", "The navigate through poses error code"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp index 4bb8ec46f7..42721f6266 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp @@ -34,14 +34,14 @@ class AreErrorCodesPresent : public BT::ConditionNode const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf) { - getInput>("error_codes_to_check", error_codes_to_check_); //NOLINT + getInput>("error_codes_to_check", error_codes_to_check_); //NOLINT } AreErrorCodesPresent() = delete; BT::NodeStatus tick() { - getInput("error_code", error_code_); //NOLINT + getInput("error_code", error_code_); //NOLINT if (error_codes_to_check_.find(error_code_) != error_codes_to_check_.end()) { return BT::NodeStatus::SUCCESS; @@ -54,14 +54,14 @@ class AreErrorCodesPresent : public BT::ConditionNode { return { - BT::InputPort("error_code", "The active error codes"), //NOLINT - BT::InputPort>("error_codes_to_check", "Error codes to check")//NOLINT + BT::InputPort("error_code", "The active error codes"), //NOLINT + BT::InputPort>("error_codes_to_check", "Error codes to check")//NOLINT }; } protected: - unsigned short error_code_; //NOLINT - std::set error_codes_to_check_; //NOLINT + uint16_t error_code_; //NOLINT + std::set error_codes_to_check_; //NOLINT }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index b3bb99adf9..1d449dca2d 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.2.1 + 1.2.2 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index febaa7de64..1f7226e6aa 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -46,7 +46,7 @@ GoalUpdater::GoalUpdater( sub_option.callback_group = callback_group_; goal_sub_ = node_->create_subscription( goal_updater_topic, - 10, + rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goal, this, _1), sub_option); } @@ -59,8 +59,17 @@ inline BT::NodeStatus GoalUpdater::tick() callback_group_executor_.spin_some(); - if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) { - goal = last_goal_received_; + if (last_goal_received_.header.stamp != rclcpp::Time(0)) { + auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); + auto goal_time = rclcpp::Time(goal.header.stamp); + if (last_goal_received_time > goal_time) { + goal = last_goal_received_; + } else { + RCLCPP_WARN( + node_->get_logger(), "The timestamp of the received goal (%f) is older than the " + "current goal (%f). Ignoring the received goal.", + last_goal_received_time.seconds(), goal_time.seconds()); + } } setOutput("output_goal", goal); diff --git a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp index 2185b0a0d3..993f3b4147 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp @@ -27,8 +27,8 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF using ActionGoal = Action::Goal; void SetUp() { - int error_code = ActionGoal::NONE; - std::set error_codes_to_check = {ActionGoal::UNKNOWN}; //NOLINT + uint16_t error_code = ActionGoal::NONE; + std::set error_codes_to_check = {ActionGoal::UNKNOWN}; //NOLINT config_->blackboard->set("error_code", error_code); config_->blackboard->set("error_codes_to_check", error_codes_to_check); @@ -58,7 +58,7 @@ std::shared_ptr AreErrorCodesPresentFixture::tree_ = nullptr; TEST_F(AreErrorCodesPresentFixture, test_condition) { - std::map error_to_status_map = { + std::map error_to_status_map = { {ActionGoal::NONE, BT::NodeStatus::FAILURE}, {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS}, }; diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp index 79335a2ba2..ec9c6bd044 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp @@ -27,7 +27,7 @@ class WouldAControllerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorT using ActionGoal = Action::Goal; void SetUp() { - int error_code = ActionGoal::NONE; + uint16_t error_code = ActionGoal::NONE; config_->blackboard->set("error_code", error_code); std::string xml_txt = @@ -56,7 +56,7 @@ std::shared_ptr WouldAControllerRecoveryHelpFixture::tree_ = nullptr; TEST_F(WouldAControllerRecoveryHelpFixture, test_condition) { - std::map error_to_status_map = { + std::map error_to_status_map = { {ActionGoal::NONE, BT::NodeStatus::FAILURE}, {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS}, {ActionGoal::INVALID_CONTROLLER, BT::NodeStatus::FAILURE}, diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp index 4313ad0a0e..a4ccbb07de 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp @@ -27,7 +27,7 @@ class WouldAPlannerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTree using ActionGoal = Action::Goal; void SetUp() { - int error_code = ActionGoal::NONE; + uint16_t error_code = ActionGoal::NONE; config_->blackboard->set("error_code", error_code); std::string xml_txt = @@ -56,7 +56,7 @@ std::shared_ptr WouldAPlannerRecoveryHelpFixture::tree_ = nullptr; TEST_F(WouldAPlannerRecoveryHelpFixture, test_condition) { - std::map error_to_status_map = { + std::map error_to_status_map = { {ActionGoal::NONE, BT::NodeStatus::FAILURE}, {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS}, {ActionGoal::NO_VALID_PATH, BT::NodeStatus::SUCCESS}, diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp index db93a32fa3..cd697773ba 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp @@ -27,7 +27,7 @@ class WouldASmootherRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTre using ActionGoal = Action::Goal; void SetUp() { - int error_code = ActionGoal::NONE; + uint16_t error_code = ActionGoal::NONE; config_->blackboard->set("error_code", error_code); std::string xml_txt = @@ -56,7 +56,7 @@ std::shared_ptr WouldASmootherRecoveryHelpFixture::tree_ = nullptr; TEST_F(WouldASmootherRecoveryHelpFixture, test_condition) { - std::map error_to_status_map = { + std::map error_to_status_map = { {ActionGoal::NONE, BT::NodeStatus::FAILURE}, {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS}, {ActionGoal::TIMEOUT, BT::NodeStatus::SUCCESS}, diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 9dc67914bd..cc6db706f2 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -95,6 +95,8 @@ TEST_F(GoalUpdaterTestFixture, test_tick) )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto goal_updater_pub = + node_->create_publisher("goal_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; @@ -102,35 +104,92 @@ TEST_F(GoalUpdaterTestFixture, test_tick) goal.pose.position.x = 1.0; config_->blackboard->set("goal", goal); - // tick until node succeeds - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { - tree_->rootNode()->executeTick(); - } - + // tick tree without publishing updated goal and get updated_goal + tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; config_->blackboard->get("updated_goal", updated_goal); +} - EXPECT_EQ(updated_goal, goal); +TEST_F(GoalUpdaterTestFixture, test_older_goal_update) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto goal_updater_pub = + node_->create_publisher("goal_update", 10); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update; - goal_to_update.header.stamp = node_->now(); + goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0); goal_to_update.pose.position.x = 2.0; + goal_updater_pub->publish(goal_to_update); + tree_->rootNode()->executeTick(); + geometry_msgs::msg::PoseStamped updated_goal; + config_->blackboard->get("updated_goal", updated_goal); + + // expect to succeed and not update goal + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(updated_goal, goal); +} + +TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); auto goal_updater_pub = node_->create_publisher("goal_update", 10); - auto start = node_->now(); - while ((node_->now() - start).seconds() < 0.5) { - tree_->rootNode()->executeTick(); - goal_updater_pub->publish(goal_to_update); + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); - rclcpp::spin_some(node_); - } + // publish updated_goal older than goal + geometry_msgs::msg::PoseStamped goal_to_update_1; + goal_to_update_1.header.stamp = node_->now(); + goal_to_update_1.pose.position.x = 2.0; + + geometry_msgs::msg::PoseStamped goal_to_update_2; + goal_to_update_2.header.stamp = node_->now(); + goal_to_update_2.pose.position.x = 3.0; + goal_updater_pub->publish(goal_to_update_1); + goal_updater_pub->publish(goal_to_update_2); + tree_->rootNode()->executeTick(); + geometry_msgs::msg::PoseStamped updated_goal; config_->blackboard->get("updated_goal", updated_goal); - EXPECT_NE(updated_goal, goal); - EXPECT_EQ(updated_goal, goal_to_update); + // expect to succeed + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + // expect to update goal with latest goal update + EXPECT_EQ(updated_goal, goal_to_update_2); } int main(int argc, char ** argv) diff --git a/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp similarity index 100% rename from nav2_behaviors/plugins/assisted_teleop.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp diff --git a/nav2_behaviors/plugins/back_up.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/back_up.hpp similarity index 100% rename from nav2_behaviors/plugins/back_up.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/back_up.hpp diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp similarity index 100% rename from nav2_behaviors/plugins/drive_on_heading.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp diff --git a/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp similarity index 100% rename from nav2_behaviors/plugins/spin.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp diff --git a/nav2_behaviors/plugins/wait.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp similarity index 100% rename from nav2_behaviors/plugins/wait.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 66048a4a68..10311c5970 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.2.1 + 1.2.2 TODO Carlos Orduno Steve Macenski diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index dee7f7232f..4ec5610208 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -14,7 +14,7 @@ #include -#include "assisted_teleop.hpp" +#include "nav2_behaviors/plugins/assisted_teleop.hpp" #include "nav2_util/node_utils.hpp" namespace nav2_behaviors diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index a72a0c3830..7c54aa0b3c 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "back_up.hpp" +#include "nav2_behaviors/plugins/back_up.hpp" namespace nav2_behaviors { diff --git a/nav2_behaviors/plugins/drive_on_heading.cpp b/nav2_behaviors/plugins/drive_on_heading.cpp index 53525b3bb6..9b44f0bacb 100644 --- a/nav2_behaviors/plugins/drive_on_heading.cpp +++ b/nav2_behaviors/plugins/drive_on_heading.cpp @@ -14,7 +14,7 @@ // limitations under the License. #include -#include "drive_on_heading.hpp" +#include "nav2_behaviors/plugins/drive_on_heading.hpp" #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_behaviors::DriveOnHeading<>, nav2_core::Behavior) diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index ad10fc1d0c..44093962b6 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -18,7 +18,7 @@ #include #include -#include "spin.hpp" +#include "nav2_behaviors/plugins/spin.hpp" #include "tf2/utils.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_util/node_utils.hpp" diff --git a/nav2_behaviors/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp index 9b006196d9..e47de4a8f8 100644 --- a/nav2_behaviors/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -15,7 +15,7 @@ #include #include -#include "wait.hpp" +#include "nav2_behaviors/plugins/wait.hpp" namespace nav2_behaviors { diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 8fddcc3253..24668627c8 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -24,6 +24,7 @@ from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ParameterFile from nav2_common.launch import RewrittenYaml @@ -53,11 +54,13 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites={}, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True), + allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 91fd42b1eb..9a0219c885 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -21,11 +21,11 @@ from launch.actions import SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import EqualsSubstitution -from launch.substitutions import NotEqualsSubstitution from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import NotEqualsSubstitution from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -55,11 +55,13 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites={}, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True), + allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index f4a67e4d6e..f1f17c50e6 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -22,7 +22,7 @@ from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -61,11 +61,13 @@ def generate_launch_description(): param_substitutions = { 'autostart': autostart} - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 9454441558..c87ae01b41 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -71,7 +71,8 @@ def generate_launch_description(): namespace=namespace, arguments=['-d', namespaced_rviz_config_file], output='screen', - remappings=[('/tf', 'tf'), + remappings=[('/map', 'map'), + ('/tf', 'tf'), ('/tf_static', 'tf_static'), ('/goal_pose', 'goal_pose'), ('/clicked_point', 'clicked_point'), diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index 5bbbd43247..abb3c13d62 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -22,6 +22,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node, SetParameter +from launch_ros.descriptions import ParameterFile from nav2_common.launch import HasNodeParams, RewrittenYaml @@ -43,11 +44,13 @@ def generate_launch_description(): slam_launch_file = os.path.join(slam_toolbox_dir, 'launch', 'online_sync_launch.py') # Create our own temporary YAML files that include substitutions - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites={}, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True), + allow_substs=True) # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index a6b0b652ba..b3fb936a3f 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.1 + 1.2.2 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 5827fbfb93..46c006a7fe 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.2.1 + 1.2.2 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index afae6ca085..cbd686cc04 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -203,8 +203,15 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) void NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) { + geometry_msgs::msg::PoseStamped current_pose; + nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance); + RCLCPP_INFO( - logger_, "Begin navigating from current location to (%.2f, %.2f)", + logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)", + current_pose.pose.position.x, current_pose.pose.position.y, goal->pose.pose.position.x, goal->pose.pose.position.y); // Reset state for new action feedback diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index 8811667458..3d0006ac85 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -26,7 +26,7 @@ from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node from launch_ros.actions import PushRosNamespace -from launch_ros.descriptions import ComposableNode +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -73,11 +73,13 @@ def generate_launch_description(): 'container_name', default_value='nav2_container', description='the name of conatiner that nodes will load in if use composition') - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites={}, - convert_types=True) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True), + allow_substs=True) # Declare node launching commands load_nodes = GroupAction( diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 11f838d1c0..b7f2439b5b 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.2.1 + 1.2.2 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index ffc7981e2a..239960e593 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -34,7 +34,7 @@ collision_monitor: type: "polygon" points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5] action_type: "limit" - max_points: 3 + min_points: 4 linear_limit: 0.4 angular_limit: 0.5 visualize: True diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 9b00d45834..2bae91e2a0 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -21,6 +21,7 @@ #include "tf2_ros/create_timer_ros.h" #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" #include "nav2_collision_monitor/kinematics.hpp" @@ -160,6 +161,12 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/) void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg) { + // If message contains NaN or Inf, ignore + if (!nav2_util::validateTwist(*msg)) { + RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!"); + return; + } + process({msg->linear.x, msg->linear.y, msg->angular.z}); } diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 556e35da46..cd070d70be 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -79,7 +79,7 @@ void Range::getData( // Ignore data, if its range is out of scope of range sensor abilities if (data_->range < data_->min_range || data_->range > data_->max_range) { - RCLCPP_WARN( + RCLCPP_DEBUG( logger_, "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", source_name_.c_str(), data_->range, data_->min_range, data_->max_range); diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake index a49fc15d7d..3f4977193c 100644 --- a/nav2_common/cmake/nav2_package.cmake +++ b/nav2_common/cmake/nav2_package.cmake @@ -36,8 +36,9 @@ macro(nav2_package) endif() endif() - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wnon-virtual-dtor) + if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) + add_compile_options("$<$:-Wnon-virtual-dtor>") endif() option(COVERAGE_ENABLED "Enable code coverage" FALSE) diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 3b29fdda59..2887fa5503 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -128,17 +128,21 @@ def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): yaml[key] = rewrite_val break key = yaml_key_list.pop(0) - yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) - + if isinstance(yaml, list): + yaml[int(key)] = self.updateYamlPathVals(yaml[int(key)], yaml_key_list, rewrite_val) + else: + yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) return yaml def substitute_keys(self, yaml, key_rewrites): if len(key_rewrites) != 0: - for key, val in yaml.items(): - if isinstance(val, dict) and key in key_rewrites: + for key in list(yaml.keys()): + val = yaml[key] + if key in key_rewrites: new_key = key_rewrites[key] yaml[new_key] = yaml[key] del yaml[key] + if isinstance(val, dict): self.substitute_keys(val, key_rewrites) def getYamlLeafKeys(self, yamlData): diff --git a/nav2_common/package.xml b/nav2_common/package.xml index e48b133a5e..57fd9fd6d8 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.2.1 + 1.2.2 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt index 6bc4e7dc6e..c68732d4d7 100644 --- a/nav2_constrained_smoother/CMakeLists.txt +++ b/nav2_constrained_smoother/CMakeLists.txt @@ -27,7 +27,6 @@ set(library_name nav2_constrained_smoother) include_directories( include - ${CERES_INCLUDES} ) set(dependencies @@ -42,7 +41,7 @@ set(dependencies ) add_library(${library_name} SHARED src/constrained_smoother.cpp) -target_link_libraries(${library_name} ${CERES_LIBRARIES}) +target_link_libraries(${library_name} Ceres::ceres) # prevent pluginlib from using boost target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_target_dependencies(${library_name} ${dependencies}) diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp index aa3ba1eb02..526754e8ed 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp @@ -194,8 +194,8 @@ class Smoother // update cusp zone costmap weights if (is_cusp) { double len_to_cusp = current_segment_len; - for (int i = potential_cusp_funcs.size() - 1; i >= 0; i--) { - auto & f = potential_cusp_funcs[i]; + for (int i_cusp = potential_cusp_funcs.size() - 1; i_cusp >= 0; i_cusp--) { + auto & f = potential_cusp_funcs[i_cusp]; double new_weight = params.cusp_costmap_weight * (1.0 - len_to_cusp / cusp_half_length) + params.costmap_weight * len_to_cusp / cusp_half_length; diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index bf4f19104c..2b77f4f313 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.2.1 + 1.2.2 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 56bff4102a..911e423484 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.2.1 + 1.2.2 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index c730f252a0..e6205ad906 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.2.1 + 1.2.2 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp index ee79894847..4527e07fdd 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp @@ -999,7 +999,10 @@ class GroupsRemover // The group of pixels labeled 0 corresponds to empty map cells. // Zero bin of the histogram is equal to the number of pixels in this group. // Because the values of empty map cells should not be changed, we will reset this bin - groups_sizes.front() = 0; // don't change image background value + if (!groups_sizes.empty()) { + groups_sizes.front() = 0; // don't change image background value + } + // noise_labels_table[i] = true if group with label i is noise std::vector noise_labels_table(groups_sizes.size()); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 68ea9c8de6..d509e7093f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -188,7 +188,6 @@ class InflationLayer : public Layer */ void onFootprintChanged() override; -private: /** * @brief Lookup pre-computed distances * @param mx The x coordinate of the current cell diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp index 71cd4654a9..a4383f1477 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -128,7 +128,7 @@ class RangeSensorLayer : public CostmapLayer */ void bufferIncomingRangeMsg(const sensor_msgs::msg::Range::SharedPtr range_message); -private: +protected: /** * @brief Processes all sensors into the costmap buffered from callbacks */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index aec68fac1f..dd1a65cef0 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -123,7 +123,7 @@ class StaticLayer : public CostmapLayer */ virtual void matchSize(); -private: +protected: /** * @brief Get parameters of layer */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 14baefcbdd..888e401439 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -133,7 +133,6 @@ class VoxelLayer : public ObstacleLayer */ virtual void resetMaps(); -private: /** * @brief Use raycasting between 2 points to clear freespace */ diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index db41f9939b..d720978e51 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.2.1 + 1.2.2 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index 81bbc88a33..f05726a6b3 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -78,7 +78,7 @@ void CostmapFilter::onInitialize() // Get parameters node->get_parameter(name_ + "." + "enabled", enabled_); filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string(); - double transform_tolerance; + double transform_tolerance {}; node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); diff --git a/nav2_costmap_2d/plugins/denoise_layer.cpp b/nav2_costmap_2d/plugins/denoise_layer.cpp index b8f6c427a6..960f83be99 100644 --- a/nav2_costmap_2d/plugins/denoise_layer.cpp +++ b/nav2_costmap_2d/plugins/denoise_layer.cpp @@ -171,12 +171,12 @@ DenoiseLayer::removeSinglePixels(Image & image) const return v == NO_INFORMATION ? FREE_SPACE : v; }; auto max = [&](const std::initializer_list lst) { - std::array buf = { + std::array rbuf = { replace_to_free(*lst.begin()), replace_to_free(*(lst.begin() + 1)), replace_to_free(*(lst.begin() + 2)) }; - return *std::max_element(buf.begin(), buf.end()); + return *std::max_element(rbuf.begin(), rbuf.end()); }; dilate(image, max_neighbors_image, group_connectivity_type_, max); } else { diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index feedb552c2..0d7b442a8a 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -338,9 +338,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) costmap_publisher_.reset(); clear_costmap_service_.reset(); - for (auto & layer_pub : layer_publishers_) { - layer_pub.reset(); - } + layer_publishers_.clear(); layered_costmap_.reset(); diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 26463250a7..790466f38b 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -117,7 +117,7 @@ class LayerSubscriber protected: void layerCallback(const nav2_msgs::msg::Costmap::SharedPtr layer) { - if (!callback_hit_) { + if (!callback_hit_ && (layer->data.size() == 100)) { layer_promise_.set_value(layer); callback_hit_ = true; } @@ -157,23 +157,24 @@ TEST_F(CostmapRosTestFixture, costmap_pub_test) { auto future = layer_subscriber_->layer_promise_.get_future(); auto status = future.wait_for(std::chrono::seconds(5)); - EXPECT_TRUE(status == std::future_status::ready); + ASSERT_TRUE(status == std::future_status::ready); auto costmap_raw = future.get(); // Check first 20 cells of the 10by10 map + ASSERT_TRUE(costmap_raw->data.size() == 100); unsigned int i = 0; for (; i < 7; ++i) { - EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE); } for (; i < 10; ++i) { - EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::LETHAL_OBSTACLE); + EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::LETHAL_OBSTACLE); } for (; i < 17; ++i) { - EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE); } for (; i < 20; ++i) { - EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::LETHAL_OBSTACLE); + EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::LETHAL_OBSTACLE); } SUCCEED(); diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index cfafb6eb4e..466fd015c8 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -73,8 +73,8 @@ class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber DummyFootprintSubscriber( nav2_util::LifecycleNode::SharedPtr node, std::string & topic_name, - tf2_ros::Buffer & tf_) - : FootprintSubscriber(node, topic_name, tf_) + tf2_ros::Buffer & tf) + : FootprintSubscriber(node, topic_name, tf) {} void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg) diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 705ed7dca8..c219b78f55 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.2.1 + 1.2.2 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index d18f934785..d3cac0e84d 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.2.1 + 1.2.2 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 3006eb6f77..64757f3169 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -526,8 +526,8 @@ DWBLocalPlanner::transformGlobalPlan( // from the robot using integrated distance auto transformation_end = std::find_if( transformation_begin, global_plan_.poses.end(), - [&](const auto & pose) { - return euclidean_distance(pose, robot_pose.pose) > transform_end_threshold; + [&](const auto & global_plan_pose) { + return euclidean_distance(global_plan_pose, robot_pose.pose) > transform_end_threshold; }); // Transform the near part of the global plan into the robot's frame of reference. diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 89874781e8..6fed02f0ce 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.2.1 + 1.2.2 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp index 30d8f47c6a..b556a06be1 100644 --- a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp +++ b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp @@ -105,8 +105,7 @@ TEST(ObstacleFootprint, GetOrientedFootprint) pose.theta = theta; footprint_after = dwb_critics::getOrientedFootprint(pose, footprint_before); - uint i; - for (i = 0; i < footprint_before.size(); i++) { + for (uint i = 0; i < footprint_before.size(); i++) { ASSERT_EQ(rotate_origin(footprint_before[i], theta), footprint_after[i]); } diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 92c9dcb374..4382f87c61 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.2.1 + 1.2.2 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index adcfe44ad8..6cc98d1d7f 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.2.1 + 1.2.2 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index b806399bfd..edf48aec1d 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.2.1 + 1.2.2 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 57650cefa6..c84bb28975 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.2.1 + 1.2.2 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index e85f5789b4..a41b6c9e45 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.2.1 + 1.2.2 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 51a726465f..9aa6059df7 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.2.1 + 1.2.2 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 16d9e1313c..fa99a5c97d 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.2.1 + 1.2.2 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index bfc81c91a8..2557bcd7d6 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -64,6 +64,9 @@ This process is then repeated a number of times and returns a converged solution | max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. | | prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. | | transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with TF. | + | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | + | inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. | + | inversion_yaw_tolerance | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg. | #### Ackermann Motion Model | Parameter | Type | Definition | @@ -81,14 +84,14 @@ This process is then repeated a number of times and returns a converged solution | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 3.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.40. Minimal distance between robot and goal above which angle goal cost considered. | + | threshold_to_consider | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered. | #### Goal Critic | Parameter | Type | Definition | | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 1.0. Distance between robot and goal above which goal cost starts being considered | + | threshold_to_consider | double | Default 1.4. Distance between robot and goal above which goal cost starts being considered | #### Obstacles Critic @@ -107,20 +110,22 @@ This process is then repeated a number of times and returns a converged solution | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 10.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path align cost stops being considered | + | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path align cost stops being considered | | offset_from_furthest | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading. | | trajectory_point_step | double | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. | | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | - + | use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. | #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 2.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path angle cost stops being considered | + | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered | | offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. | | max_angle_to_furthest | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered | + | mode | int | Default 0 (Forward Preference). Enum type for mode of operations for the path angle critic depending on path input types and behavioral desires. 0: Forward Preference, penalizes high path angles relative to the robot's orientation to incentivize turning towards the path. 1: No directional preference, penalizes high path angles relative to the robot's orientation or mirrored orientation (e.g. reverse), which ever is less, when a particular direction of travel is not preferable. 2: Consider feasible path orientation, when using a feasible path whereas the path points have orientation information (e.g. Smac Planners), consider the path's requested direction of travel to penalize path angles such that the robot will follow the path in the requested direction. | + #### Path Follow Critic | Parameter | Type | Definition | @@ -128,14 +133,14 @@ This process is then repeated a number of times and returns a converged solution | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | offset_from_furthest | int | Default 6. Number of path points after furthest one any trajectory achieves to drive path tracking relative to. | - | threshold_to_consider | float | Default 0.4. Distance between robot and goal above which path follow cost stops being considered | + | threshold_to_consider | float | Default 1.4. Distance between robot and goal above which path follow cost stops being considered | #### Prefer Forward Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which prefer forward cost stops being considered | + | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which prefer forward cost stops being considered | #### Twirling Critic @@ -182,17 +187,17 @@ controller_server: enabled: true cost_power: 1 cost_weight: 5.0 - threshold_to_consider: 1.0 + threshold_to_consider: 1.4 GoalAngleCritic: enabled: true cost_power: 1 cost_weight: 3.0 - threshold_to_consider: 0.4 + threshold_to_consider: 0.5 PreferForwardCritic: enabled: true cost_power: 1 cost_weight: 5.0 - threshold_to_consider: 0.4 + threshold_to_consider: 0.5 ObstaclesCritic: enabled: true cost_power: 1 @@ -208,21 +213,23 @@ controller_server: cost_weight: 14.0 max_path_occupancy_ratio: 0.05 trajectory_point_step: 3 - threshold_to_consider: 0.40 + threshold_to_consider: 0.5 offset_from_furthest: 20 + use_path_orientations: false PathFollowCritic: enabled: true cost_power: 1 cost_weight: 5.0 offset_from_furthest: 5 - threshold_to_consider: 0.6 + threshold_to_consider: 1.4 PathAngleCritic: enabled: true cost_power: 1 cost_weight: 2.0 offset_from_furthest: 4 - threshold_to_consider: 0.40 + threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 + forward_preference: true # TwirlingCritic: # enabled: true # twirling_cost_power: 1 @@ -251,7 +258,7 @@ If you don't require path following behavior (e.g. just want to follow a goal po As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. -The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. +The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. Setting the threshold for consideration in the path follower + goal critics as the same as your prediction horizon can make sure you have clean hand-offs between them, as the path follower will otherwise attempt to slow slightly once it reaches the final goal pose as its marker. The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic `offset_from_furthest` represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp index 62d6bd1042..8fb8fb688a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index 7618afb9a4..a549995557 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -49,6 +49,7 @@ class PathAlignCritic : public CriticFunction int trajectory_point_step_{0}; float threshold_to_consider_{0}; float max_path_occupancy_ratio_{0}; + bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index 92130dceb3..f7ba486a0c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -15,13 +15,41 @@ #ifndef NAV2_MPPI_CONTROLLER__CRITICS__PATH_ANGLE_CRITIC_HPP_ #define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ANGLE_CRITIC_HPP_ +#include #include "nav2_mppi_controller/critic_function.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" +#include "nav2_core/controller_exceptions.hpp" namespace mppi::critics { +/** + * @brief Enum type for different modes of operation + */ +enum class PathAngleMode +{ + FORWARD_PREFERENCE = 0, + NO_DIRECTIONAL_PREFERENCE = 1, + CONSIDER_FEASIBLE_PATH_ORIENTATIONS = 2 +}; + +/** + * @brief Method to convert mode enum to string for printing + */ +std::string modeToStr(const PathAngleMode & mode) +{ + if (mode == PathAngleMode::FORWARD_PREFERENCE) { + return "Forward Preference"; + } else if (mode == PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS) { + return "Consider Feasible Path Orientations"; + } else if (mode == PathAngleMode::NO_DIRECTIONAL_PREFERENCE) { + return "No Directional Preference"; + } else { + return "Invalid mode!"; + } +} + /** * @class mppi::critics::ConstraintCritic * @brief Critic objective function for aligning to path in cases of extreme misalignment @@ -48,6 +76,8 @@ class PathAngleCritic : public CriticFunction float threshold_to_consider_{0}; size_t offset_from_furthest_{0}; + bool reversing_allowed_{true}; + PathAngleMode mode_{0}; unsigned int power_{0}; float weight_{0}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index 12317c7b61..c06fba5b89 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index c66d32257e..468e4964a4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -1,4 +1,6 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -124,10 +126,18 @@ class PathHandler const geometry_msgs::msg::PoseStamped & global_pose); /** - * @brief Prune global path to only interesting portions + * @brief Prune a path to only interesting portions + * @param plan Plan to prune * @param end Final path iterator */ - void pruneGlobalPlan(const PathIterator end); + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + + /** + * @brief Check if the robot pose is within the set inversion tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the set inversion tolerances + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); std::string name_; std::shared_ptr costmap_; @@ -135,11 +145,16 @@ class PathHandler ParametersHandler * parameters_handler_; nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; double max_robot_pose_search_dist_{0}; double prune_distance_{0}; double transform_tolerance_{0}; + double inversion_xy_tolerance_{0.2}; + double inversion_yaw_tolerance{0.4}; + bool enforce_path_inversion_{false}; + unsigned int inversion_locale_{0u}; }; } // namespace mppi diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 7319e318f0..ce2d0570c0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -416,15 +417,50 @@ inline void setPathCostsIfNotSet( * @param pose pose * @param point_x Point to find angle relative to X axis * @param point_y Point to find angle relative to Y axis + * @param forward_preference If reversing direction is valid * @return Angle between two points */ -inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point_x, double point_y) +inline double posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) { double pose_x = pose.position.x; double pose_y = pose.position.y; double pose_yaw = tf2::getYaw(pose.orientation); double yaw = atan2(point_y - pose_y, point_x - pose_x); + + // If no preference for forward, return smallest angle either in heading or 180 of heading + if (!forward_preference) { + return std::min( + abs(angles::shortest_angular_distance(yaw, pose_yaw)), + abs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); + } + + return abs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief evaluate angle from pose (have angle) to point (no angle) + * @param pose pose + * @param point_x Point to find angle relative to X axis + * @param point_y Point to find angle relative to Y axis + * @param point_yaw Yaw of the point to consider along Z axis + * @return Angle between two points + */ +inline double posePointAngle( + const geometry_msgs::msg::Pose & pose, + double point_x, double point_y, double point_yaw) +{ + double pose_x = pose.position.x; + double pose_y = pose.position.y; + double pose_yaw = tf2::getYaw(pose.orientation); + + double yaw = atan2(point_y - pose_y, point_x - pose_x); + + if (abs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) { + yaw = angles::normalize_angle(yaw + M_PI); + } + return abs(angles::shortest_angular_distance(yaw, pose_yaw)); } @@ -599,6 +635,59 @@ inline void savitskyGolayFilter( control_sequence.wz(offset)}; } +/** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @param path to check for inversion + * @return the first point after the inversion found in the path + */ +inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + // Iterating through the path to determine the position of the path inversion + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + double oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + double ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + double ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existance of cusp, in the path, using the dot product. + double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + return idx + 1; + } + } + + return path.poses.size(); +} + +/** + * @brief Find and remove poses after the first inversion in the path + * @param path to check for inversion + * @return The location of the inversion, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +{ + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); + if (first_after_inversion == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + path = cropped_path; + return first_after_inversion; +} + } // namespace mppi::utils #endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 8af09f4464..cd68d15b5f 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.2.1 + 1.2.2 nav2_mppi_controller Aleksei Budyakov Steve Macenski diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index fc26f89d11..4dc2bcf1de 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -28,15 +28,14 @@ void ConstraintCritic::initialize() logger_, "ConstraintCritic instantiated with %d power and %f weight.", power_, weight_); - float vx_max, vy_max, vx_min, vy_min; + float vx_max, vy_max, vx_min; getParentParam(vx_max, "vx_max", 0.5); getParentParam(vy_max, "vy_max", 0.0); getParentParam(vx_min, "vx_min", -0.35); - getParentParam(vy_min, "vy_min", 0.0); const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0; max_vel_ = std::sqrt(vx_max * vx_max + vy_max * vy_max); - min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_min * vy_min); + min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_max * vy_max); } void ConstraintCritic::score(CriticData & data) diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index c196871327..62bfdf0676 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -24,7 +24,7 @@ void GoalAngleCritic::initialize() getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.0); - getParam(threshold_to_consider_, "threshold_to_consider", 0.40); + getParam(threshold_to_consider_, "threshold_to_consider", 0.5); RCLCPP_INFO( logger_, diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 859a9c3f5c..e38a98ed6b 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,13 +18,15 @@ namespace mppi::critics { +using xt::evaluation_strategy::immediate; + void GoalCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0); - getParam(threshold_to_consider_, "threshold_to_consider", 1.0); + getParam(threshold_to_consider_, "threshold_to_consider", 1.4); RCLCPP_INFO( logger_, "GoalCritic instantiated with %d power and %f weight.", @@ -47,14 +50,14 @@ void GoalCritic::score(CriticData & data) const auto goal_x = data.path.x(goal_idx); const auto goal_y = data.path.y(goal_idx); - const auto last_x = xt::view(data.trajectories.x, xt::all(), -1); - const auto last_y = xt::view(data.trajectories.y, xt::all(), -1); + const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all()); + const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all()); auto dists = xt::sqrt( - xt::pow(last_x - goal_x, 2) + - xt::pow(last_y - goal_y, 2)); + xt::pow(traj_x - goal_x, 2) + + xt::pow(traj_y - goal_y, 2)); - data.costs += xt::pow(std::move(dists) * weight_, power_); + data.costs += xt::pow(xt::mean(dists, {1}) * weight_, power_); } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index f45ce72116..dcdb08f614 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -31,6 +31,17 @@ void ObstaclesCritic::initialize() collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + + if (possibly_inscribed_cost_ < 1) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + RCLCPP_INFO( logger_, "ObstaclesCritic instantiated with %d power and %f / %f weights. " @@ -183,7 +194,7 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) collision_checker_.worldToMap(x, y, x_i, y_i); cost = collision_checker_.pointCost(x_i, y_i); - if (consider_footprint_ && cost >= possibly_inscribed_cost_) { + if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) { cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); collision_cost.using_footprint = true; diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index b95b96a04a..2585193ade 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -34,7 +34,8 @@ void PathAlignCritic::initialize() getParam(trajectory_point_step_, "trajectory_point_step", 4); getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 0.5); + getParam(use_path_orientations_, "use_path_orientations", false); RCLCPP_INFO( logger_, @@ -71,9 +72,11 @@ void PathAlignCritic::score(CriticData & data) const auto & T_x = data.trajectories.x; const auto & T_y = data.trajectories.y; + const auto & T_yaw = data.trajectories.yaws; const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points + const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points const size_t batch_size = T_x.shape(0); const size_t time_steps = T_x.shape(1); @@ -85,18 +88,27 @@ void PathAlignCritic::score(CriticData & data) return; } + float dist_sq = 0, dx = 0, dy = 0, dyaw = 0, summed_dist = 0; + double min_dist_sq = std::numeric_limits::max(); + size_t min_s = 0; + for (size_t t = 0; t < batch_size; ++t) { - float summed_dist = 0; + summed_dist = 0; for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { - double min_dist_sq = std::numeric_limits::max(); - size_t min_s = 0; + min_dist_sq = std::numeric_limits::max(); + min_s = 0; // Find closest path segment to the trajectory point for (size_t s = 0; s < path_segments_count - 1; s++) { xt::xtensor_fixed> P; - float dx = P_x(s) - T_x(t, p); - float dy = P_y(s) - T_y(t, p); - float dist_sq = dx * dx + dy * dy; + dx = P_x(s) - T_x(t, p); + dy = P_y(s) - T_y(t, p); + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); + dist_sq = dx * dx + dy * dy + dyaw * dyaw; + } else { + dist_sq = dx * dx + dy * dy; + } if (dist_sq < min_dist_sq) { min_dist_sq = dist_sq; min_s = s; diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index a588bbe291..cd701ea093 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,56 +20,117 @@ namespace mppi::critics { +using xt::evaluation_strategy::immediate; + void PathAngleCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + float vx_min; + getParentParam(vx_min, "vx_min", -0.35); + if (fabs(vx_min) < 1e-6) { // zero + reversing_allowed_ = false; + } else if (vx_min < 0.0) { // reversing possible + reversing_allowed_ = true; + } + auto getParam = parameters_handler_->getParamGetter(name_); getParam(offset_from_furthest_, "offset_from_furthest", 4); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 2.0); getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 0.5); getParam( max_angle_to_furthest_, "max_angle_to_furthest", 1.2); + int mode = 0; + getParam(mode, "mode", mode); + mode_ = static_cast(mode); + if (!reversing_allowed_ && mode_ == PathAngleMode::NO_DIRECTIONAL_PREFERENCE) { + mode_ = PathAngleMode::FORWARD_PREFERENCE; + RCLCPP_WARN( + logger_, + "Path angle mode set to no directional preference, but controller's settings " + "don't allow for reversing! Setting mode to forward preference."); + } RCLCPP_INFO( logger_, - "PathAngleCritic instantiated with %d power and %f weight.", - power_, weight_); + "PathAngleCritic instantiated with %d power and %f weight. Mode set to: %s", + power_, weight_, modeToStr(mode_).c_str()); } void PathAngleCritic::score(CriticData & data) { - using xt::evaluation_strategy::immediate; - if (!enabled_) { - return; - } - - if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { return; } utils::setPathFurthestPointIfNotSet(data); - auto offseted_idx = std::min( *data.furthest_reached_path_point + offset_from_furthest_, data.path.x.shape(0) - 1); const float goal_x = xt::view(data.path.x, offseted_idx); const float goal_y = xt::view(data.path.y, offseted_idx); - - if (utils::posePointAngle(data.state.pose.pose, goal_x, goal_y) < max_angle_to_furthest_) { - return; + const float goal_yaw = xt::view(data.path.yaws, offseted_idx); + const geometry_msgs::msg::Pose & pose = data.state.pose.pose; + + switch (mode_) { + case PathAngleMode::FORWARD_PREFERENCE: + if (utils::posePointAngle(pose, goal_x, goal_y, true) < max_angle_to_furthest_) { + return; + } + break; + case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: + if (utils::posePointAngle(pose, goal_x, goal_y, false) < max_angle_to_furthest_) { + return; + } + break; + case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS: + if (utils::posePointAngle(pose, goal_x, goal_y, goal_yaw) < max_angle_to_furthest_) { + return; + } + break; + default: + throw nav2_core::ControllerException("Invalid path angle mode!"); } - const auto yaws_between_points = xt::atan2( + auto yaws_between_points = xt::atan2( goal_y - data.trajectories.y, goal_x - data.trajectories.x); - const auto yaws = + + auto yaws = xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points)); - data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + switch (mode_) { + case PathAngleMode::FORWARD_PREFERENCE: + { + data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + return; + } + case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: + { + const auto yaws_between_points_corrected = xt::where( + yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); + const auto corrected_yaws = xt::abs( + utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); + data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); + return; + } + case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS: + { + const auto yaws_between_points_corrected = xt::where( + xt::abs(utils::shortest_angular_distance(yaws_between_points, goal_yaw)) < M_PI_2, + yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); + const auto corrected_yaws = xt::abs( + utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); + data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); + return; + } + } } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index e440bd0b3e..01ecb91728 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -26,7 +26,7 @@ void PathFollowCritic::initialize() getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 1.4); getParam(offset_from_furthest_, "offset_from_furthest", 6); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0); diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 5cea014bbc..18b6900177 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -24,7 +24,7 @@ void PreferForwardCritic::initialize() getParam(weight_, "cost_weight", 5.0); getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 0.5); RCLCPP_INFO( logger_, "PreferForwardCritic instantiated with %d power and %f weight.", power_, weight_); diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 6fe71eada5..f1022513f2 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -1,4 +1,6 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,6 +37,12 @@ void PathHandler::initialize( getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist()); getParam(prune_distance_, "prune_distance", 1.5); getParam(transform_tolerance_, "transform_tolerance", 0.1); + getParam(enforce_path_inversion_, "enforce_path_inversion", false); + if (enforce_path_inversion_) { + getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2); + getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4); + inversion_locale_ = 0u; + } } std::pair @@ -43,12 +51,13 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( { using nav2_util::geometry_utils::euclidean_distance; - auto begin = global_plan_.poses.begin(); + auto begin = global_plan_up_to_inversion_.poses.begin(); // Limit the search for the closest pose up to max_robot_pose_search_dist on the path auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); // Find closest point to the robot auto closest_point = nav2_util::geometry_utils::min_by( @@ -63,7 +72,7 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( auto pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( - closest_point, global_plan_.poses.end(), prune_distance_); + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); unsigned int mx, my; // Find the furthest relevent pose on the path to consider within costmap @@ -95,12 +104,12 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( const geometry_msgs::msg::PoseStamped & pose) { - if (global_plan_.poses.empty()) { + if (global_plan_up_to_inversion_.poses.empty()) { throw nav2_core::InvalidPath("Received plan with zero length"); } geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) { throw nav2_core::ControllerTFError( "Unable to transform robot pose into global plan's frame"); } @@ -116,7 +125,15 @@ nav_msgs::msg::Path PathHandler::transformPath( transformToGlobalPlanFrame(robot_pose); auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); - pruneGlobalPlan(lower_bound); + prunePlan(global_plan_up_to_inversion_, lower_bound); + + if (enforce_path_inversion_ && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(global_pose)) { + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } + } if (transformed_plan.poses.empty()) { throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); @@ -156,13 +173,32 @@ double PathHandler::getMaxCostmapDist() void PathHandler::setPath(const nav_msgs::msg::Path & plan) { global_plan_ = plan; + global_plan_up_to_inversion_ = global_plan_; + if (enforce_path_inversion_) { + inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } } nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} -void PathHandler::pruneGlobalPlan(const PathIterator end) +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) { - global_plan_.poses.erase(global_plan_.poses.begin(), end); + plan.poses.erase(plan.poses.begin(), end); +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + double distance = std::hypot( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + double angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance; } } // namespace mppi diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index 5a931de92f..f24d205bfb 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -65,12 +65,22 @@ class CriticManagerWrapper : public CriticManager bool getDummyCriticInitialized() { - return dynamic_cast(critics_[0].get())->initialized_; + const auto critic = critics_[0].get(); + if (critic == nullptr) { + return false; + } + const auto dummy_critic = dynamic_cast(critic); + return dummy_critic == nullptr ? false : dummy_critic->initialized_; } bool getDummyCriticScored() { - return dynamic_cast(critics_[0].get())->scored_; + const auto critic = critics_[0].get(); + if (critic == nullptr) { + return false; + } + const auto dummy_critic = dynamic_cast(critic); + return dummy_critic == nullptr ? false : dummy_critic->scored_; } }; diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index d574cd769e..ce2896122c 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -39,6 +40,20 @@ using namespace mppi::critics; // NOLINT using namespace mppi::utils; // NOLINT using xt::evaluation_strategy::immediate; +class PathAngleCriticWrapper : public PathAngleCritic +{ +public: + PathAngleCriticWrapper() + : PathAngleCritic() + { + } + + void setMode(int mode) + { + mode_ = static_cast(mode); + } +}; + TEST(CriticTests, ConstraintsCritic) { // Standard preamble @@ -239,7 +254,7 @@ TEST(CriticTests, PathAngleCritic) // Initialization testing // Make sure initializes correctly - PathAngleCritic critic; + PathAngleCriticWrapper critic; critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); EXPECT_EQ(critic.getName(), "critic"); @@ -267,6 +282,64 @@ TEST(CriticTests, PathAngleCritic) critic.score(data); EXPECT_GT(xt::sum(costs, immediate)(), 0.0); EXPECT_NEAR(costs(0), 3.6315, 1e-2); // atan2(4,-1) [1.81] * 2.0 weight + + // Set mode to no directional preferences + reset costs + critic.setMode(1); + costs = xt::zeros({1000}); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path pt and pose < max_angle_to_furthest_ IF non-directional + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 4.0; + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0.0); + // should use reverse orientation as the closer angle in no dir preference mode + EXPECT_NEAR(costs(0), 2.6516, 1e-2); + + // Set mode to consider path directionality + reset costs + critic.setMode(2); + costs = xt::zeros({1000}); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path pt and pose < max_angle_to_furthest_ IF non-directional + path.y(6) = 0.0; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close but outside of tol. with more than PI/2 angular diff. + path.x(6) = -1.0; // angle between path point and pose < max_angle_to_furthest_ + path.y(6) = 4.0; + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0.0); + // should use reverse orientation as the closer angle in no dir preference mode + EXPECT_NEAR(costs(0), 2.6516, 1e-2); + + PathAngleMode mode; + mode = PathAngleMode::FORWARD_PREFERENCE; + EXPECT_EQ(modeToStr(mode), std::string("Forward Preference")); + mode = PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS; + EXPECT_EQ(modeToStr(mode), std::string("Consider Feasible Path Orientations")); + mode = PathAngleMode::NO_DIRECTIONAL_PREFERENCE; + EXPECT_EQ(modeToStr(mode), std::string("No Directional Preference")); + mode = static_cast(4); + EXPECT_EQ(modeToStr(mode), std::string("Invalid mode!")); } TEST(CriticTests, PreferForwardCritic) @@ -417,9 +490,9 @@ TEST(CriticTests, PathFollowCritic) // Scoring testing // provide state poses and path close within positional tolerances - state.pose.pose.position.x = 1.0; + state.pose.pose.position.x = 2.0; path.reset(6); - path.x(5) = 0.85; + path.x(5) = 1.7; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 3eb737ed36..0bcf554392 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -38,9 +38,9 @@ class PathHandlerWrapper : public PathHandler PathHandlerWrapper() : PathHandler() {} - void pruneGlobalPlanWrapper(const PathIterator end) + void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end) { - return pruneGlobalPlan(end); + return prunePlan(path, end); } double getMaxCostmapDistWrapper() @@ -66,6 +66,21 @@ class PathHandlerWrapper : public PathHandler { return transformToGlobalPlanFrame(pose); } + + void setGlobalPlanUpToInversion(const nav_msgs::msg::Path & path) + { + global_plan_up_to_inversion_ = path; + } + + bool isWithinInversionTolerancesWrapper(const geometry_msgs::msg::PoseStamped & robot_pose) + { + return isWithinInversionTolerances(robot_pose); + } + + nav_msgs::msg::Path & getInvertedPath() + { + return global_plan_up_to_inversion_; + } }; TEST(PathHandlerTests, GetAndPrunePath) @@ -82,7 +97,7 @@ TEST(PathHandlerTests, GetAndPrunePath) EXPECT_EQ(path.poses.size(), rtn_path.poses.size()); PathIterator it = rtn_path.poses.begin() + 5; - handler.pruneGlobalPlanWrapper(it); + handler.pruneGlobalPlanWrapper(rtn_path, it); auto rtn2_path = handler.getPath(); EXPECT_EQ(rtn2_path.poses.size(), 6u); } @@ -131,10 +146,10 @@ TEST(PathHandlerTests, TestBounds) handler.setPath(path); auto [transformed_plan, closest] = handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose); - auto & path_in = handler.getPath(); - EXPECT_EQ(closest - path_in.poses.begin(), 25); - handler.pruneGlobalPlanWrapper(closest); - auto & path_pruned = handler.getPath(); + auto & path_inverted = handler.getInvertedPath(); + EXPECT_EQ(closest - path_inverted.poses.begin(), 25); + handler.pruneGlobalPlanWrapper(path_inverted, closest); + auto & path_pruned = handler.getInvertedPath(); EXPECT_EQ(path_pruned.poses.size(), 75u); } @@ -189,3 +204,46 @@ TEST(PathHandlerTests, TestTransforms) auto final_path = handler.transformPath(robot_pose); EXPECT_EQ(final_path.poses.size(), path_out.poses.size()); } + +TEST(PathHandlerTests, TestInversionToleranceChecks) +{ + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = static_cast(i); + path.poses.push_back(pose); + } + path.poses.back().pose.orientation.w = 1; + + PathHandlerWrapper handler; + handler.setGlobalPlanUpToInversion(path); + + // Not near (0,0) + geometry_msgs::msg::PoseStamped robot_pose; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Exactly on top of it + robot_pose.pose.position.x = 9; + robot_pose.pose.orientation.w = 1.0; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Laterally of it + robot_pose.pose.position.y = 9; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // On top but off angled + robot_pose.pose.position.y = 0; + robot_pose.pose.orientation.z = 0.8509035; + robot_pose.pose.orientation.w = 0.525322; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // On top but off angled within tolerances + robot_pose.pose.position.y = 0; + robot_pose.pose.orientation.w = 0.9961947; + robot_pose.pose.orientation.z = 0.0871558; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Offset spatially + off angled but both within tolerances + robot_pose.pose.position.x = 9.10; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); +} diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 54bcd4b787..ef5f2a3d59 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -195,7 +195,25 @@ TEST(UtilsTests, AnglesTests) pose.position.y = 0.0; pose.orientation.w = 1.0; double point_x = 1.0, point_y = 0.0; - EXPECT_NEAR(posePointAngle(pose, point_x, point_y), 0.0, 1e-6); + bool forward_preference = true; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + forward_preference = false; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + point_x = -1.0; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + forward_preference = true; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), M_PI, 1e-6); + + // Test point-pose angle with goal yaws + point_x = 1.0; + double point_yaw = 0.0; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), 0.0, 1e-6); + point_yaw = M_PI; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), M_PI, 1e-6); + point_yaw = 0.1; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), 0.0, 1e-3); + point_yaw = 3.04159; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, point_yaw), M_PI, 1e-3); } TEST(UtilsTests, FurthestAndClosestReachedPoint) @@ -350,7 +368,7 @@ TEST(UtilsTests, SmootherTest) // Check that path is smoother float smoothed_val, original_val; - for (unsigned int i = 0; i != noisey_sequence.vx.shape(0); i++) { + for (unsigned int i = 1; i != noisey_sequence.vx.shape(0) - 1; i++) { smoothed_val += fabs(noisey_sequence.vx(i) - 0.2); smoothed_val += fabs(noisey_sequence.vy(i) - 0.0); smoothed_val += fabs(noisey_sequence.wz(i) - 0.3); @@ -362,3 +380,66 @@ TEST(UtilsTests, SmootherTest) EXPECT_LT(smoothed_val, original_val); } + +TEST(UtilsTests, FindPathInversionTest) +{ + // Straight path, no inversions to be found + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::findFirstPathInversion(path), 10u); + + // To short to process + path.poses.erase(path.poses.begin(), path.poses.begin() + 7); + EXPECT_EQ(utils::findFirstPathInversion(path), 3u); + + // Has inversion at index 10, so should return 11 for the first point afterwards + // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 + path.poses.clear(); + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::findFirstPathInversion(path), 11u); +} + +TEST(UtilsTests, RemovePosesAfterPathInversionTest) +{ + nav_msgs::msg::Path path; + // straight path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); + + // try empty path + path.poses.clear(); + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); + + // cusping path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u); + // Check to see if removed + EXPECT_EQ(path.poses.size(), 11u); + EXPECT_EQ(path.poses.back().pose.position.x, 10); +} diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index ed1f8ca5f1..a6bb2b9388 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -7,7 +7,6 @@ geometry_msgs/PoseStamped[] poses string behavior_tree --- #result definition -std_msgs/Empty result uint16 error_code --- #feedback definition diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index b38aa0002c..9fcf8e65fd 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -7,7 +7,6 @@ geometry_msgs/PoseStamped pose string behavior_tree --- #result definition -std_msgs/Empty result uint16 error_code --- #feedback definition diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 782ac51d21..4a70fa9229 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.2.1 + 1.2.2 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 0ce9720739..598250de9f 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.2.1 + 1.2.2 TODO Steve Macenski Carlos Orduno diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 7cc1002892..8259950b03 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -421,11 +421,10 @@ inline void NavFn::updateCell(int n) { // get neighbors - float u, d, l, r; - l = potarr[n - 1]; - r = potarr[n + 1]; - u = potarr[n - nx]; - d = potarr[n + nx]; + const float l = potarr[n - 1]; + const float r = potarr[n + 1]; + const float u = potarr[n - nx]; + const float d = potarr[n + nx]; // ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n", // potarr[n], l, r, u, d); // ROS_INFO("[Update] cost: %d\n", costarr[n]); @@ -452,8 +451,8 @@ NavFn::updateCell(int n) // use quadratic approximation // might speed this up through table lookup, but still have to // do the divide - float d = dc / hf; - float v = -0.2301 * d * d + 0.5307 * d + 0.7040; + const float div = dc / hf; + const float v = -0.2301 * div * div + 0.5307 * div + 0.7040; pot = ta + hf * v; } @@ -496,11 +495,10 @@ inline void NavFn::updateCellAstar(int n) { // get neighbors - float u, d, l, r; - l = potarr[n - 1]; - r = potarr[n + 1]; - u = potarr[n - nx]; - d = potarr[n + nx]; + float l = potarr[n - 1]; + float r = potarr[n + 1]; + float u = potarr[n - nx]; + float d = potarr[n + nx]; // ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n", // potarr[n], l, r, u, d); // ROS_INFO("[Update] cost of %d: %d\n", n, costarr[n]); @@ -527,8 +525,8 @@ NavFn::updateCellAstar(int n) // use quadratic approximation // might speed this up through table lookup, but still have to // do the divide - float d = dc / hf; - float v = -0.2301 * d * d + 0.5307 * d + 0.7040; + const float div = dc / hf; + const float v = -0.2301 * div * div + 0.5307 * div + 0.7040; pot = ta + hf * v; } @@ -834,22 +832,22 @@ NavFn::calcPath(int n, int * st) // check eight neighbors to find the lowest int minc = stc; int minp = potarr[stc]; - int st = stcpx - 1; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st++; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st++; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st = stc - 1; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st = stc + 1; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st = stcnx - 1; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st++; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} - st++; - if (potarr[st] < minp) {minp = potarr[st]; minc = st;} + int sti = stcpx - 1; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti++; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti++; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti = stc - 1; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti = stc + 1; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti = stcnx - 1; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti++; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} + sti++; + if (potarr[sti] < minp) {minp = potarr[sti]; minc = sti;} stc = minc; dx = 0; dy = 0; diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index c774293fa3..57d0b51d5c 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.2.1 + 1.2.2 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 2c8daae375..ac40ab131d 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.2.1 + 1.2.2 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index f4ceec759d..90b7c4a3e0 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -77,8 +77,8 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( const double max_costmap_extent = getCostmapMaxExtent(); auto transformation_end = std::find_if( transformation_begin, global_plan_.poses.end(), - [&](const auto & pose) { - return euclidean_distance(pose, robot_pose) > max_costmap_extent; + [&](const auto & global_plan_pose) { + return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; }); // Lambda to transform a PoseStamped from global frame to local diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 5387b74f35..44e1b7a748 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.2.1 + 1.2.2 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index c683dffb63..51df97f421 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -6,7 +6,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index a711ecc77c..55e0a88d82 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.2.1 + 1.2.2 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index a8f20bad22..f7ce1d8220 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -451,7 +451,7 @@ Nav2Panel::Nav2Panel(QWidget * parent) accumulated_nav_through_poses_->addTransition(accumulatedNTPTransition); auto options = rclcpp::NodeOptions().arguments( - {"--ros-args --remap __node:=navigation_dialog_action_client"}); + {"--ros-args", "--remap", "__node:=rviz_navigation_dialog_action_client", "--"}); client_node_ = std::make_shared("_", options); client_nav_ = std::make_shared( diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 05e953231b..abadd764fc 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.2.1 + 1.2.2 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index e9a6861f05..5f437ce546 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -126,7 +126,7 @@ planner_server: cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. - viz_expansions: True # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance. + debug_visualizations: True # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. smoother: max_iterations: 1000 w_smooth: 0.3 diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index e6520f4bd2..5933a6dc92 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -14,6 +14,7 @@ #include #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_smac_planner/constants.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #ifndef NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ #define NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ @@ -34,11 +35,13 @@ class GridCollisionChecker * for use when regular bin intervals are appropriate * @param costmap The costmap to collision check against * @param num_quantizations The number of quantizations to precompute footprint + * @param node Node to extract clock and logger from * orientations for to speed up collision checking */ GridCollisionChecker( nav2_costmap_2d::Costmap2D * costmap, - unsigned int num_quantizations); + unsigned int num_quantizations, + rclcpp_lifecycle::LifecycleNode::SharedPtr node); /** * @brief A constructor for nav2_smac_planner::GridCollisionChecker @@ -117,6 +120,8 @@ class GridCollisionChecker bool footprint_is_radius_; std::vector angles_; double possible_inscribed_cost_{-1}; + rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; + rclcpp::Clock::SharedPtr clock_; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index f944e3dac1..fae139aa2f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -117,10 +117,12 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner double _max_planning_time; double _lookup_table_size; double _minimum_turning_radius_global_coords; - bool _viz_expansions; + bool _debug_visualizations; std::string _motion_model_for_search; MotionModel _motion_model; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _planned_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; std::mutex _mutex; diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp index fcb267e980..277edb6083 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2021, Samsung Research America +// Copyright (c) 2023, Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,13 +18,18 @@ #include #include +#include +#include "nlohmann/json.hpp" #include "Eigen/Core" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose.hpp" #include "tf2/utils.h" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "nav2_smac_planner/types.hpp" +#include namespace nav2_smac_planner { @@ -154,6 +160,76 @@ inline void fromJsonToMotionPrimitive( } } +/** + * @brief transform footprint into edges + * @param[in] robot position , orientation and footprint + * @param[out] robot footprint edges + */ +inline std::vector transformFootprintToEdges( + const geometry_msgs::msg::Pose & pose, + const std::vector & footprint) +{ + const double & x = pose.position.x; + const double & y = pose.position.y; + const double & yaw = tf2::getYaw(pose.orientation); + + std::vector out_footprint; + out_footprint.resize(2 * footprint.size()); + for (unsigned int i = 0; i < footprint.size(); i++) { + out_footprint[2 * i].x = x + cos(yaw) * footprint[i].x - sin(yaw) * footprint[i].y; + out_footprint[2 * i].y = y + sin(yaw) * footprint[i].x + cos(yaw) * footprint[i].y; + if (i == 0) { + out_footprint.back().x = out_footprint[i].x; + out_footprint.back().y = out_footprint[i].y; + } else { + out_footprint[2 * i - 1].x = out_footprint[2 * i].x; + out_footprint[2 * i - 1].y = out_footprint[2 * i].y; + } + } + return out_footprint; +} + +/** + * @brief initializes marker to visualize shape of linestring + * @param edge edge to mark of footprint + * @param i marker ID + * @param frame_id frame of the marker + * @param timestamp timestamp of the marker + * @return marker populated + */ +inline visualization_msgs::msg::Marker createMarker( + const std::vector edge, + unsigned int i, const std::string & frame_id, const rclcpp::Time & timestamp) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.frame_locked = false; + marker.ns = "planned_footprint"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.lifetime = rclcpp::Duration(0, 0); + + marker.id = i; + for (auto & point : edge) { + marker.points.push_back(point); + } + + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.05; + marker.scale.y = 0.05; + marker.scale.z = 0.05; + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 1.0f; + marker.color.a = 1.3f; + return marker; +} + + } // namespace nav2_smac_planner #endif // NAV2_SMAC_PLANNER__UTILS_HPP_ diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index a006442220..69ef0f7950 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.2.1 + 1.2.2 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index c0cbfb98fa..20d288809d 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -19,9 +19,15 @@ namespace nav2_smac_planner GridCollisionChecker::GridCollisionChecker( nav2_costmap_2d::Costmap2D * costmap, - unsigned int num_quantizations) + unsigned int num_quantizations, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) : FootprintCollisionChecker(costmap) { + if (node) { + clock_ = node->get_clock(); + logger_ = node->get_logger(); + } + // Convert number of regular bins into angles float bin_size = 2 * M_PI / static_cast(num_quantizations); angles_.reserve(num_quantizations); @@ -104,7 +110,17 @@ bool GridCollisionChecker::inCollision( static_cast(x), static_cast(y)); if (footprint_cost_ < possible_inscribed_cost_) { - return false; + if (possible_inscribed_cost_ > 0) { + return false; + } else { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } } // If its inscribed, in collision, or unknown in the middle, diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 46dfa98fb2..94b8ee1306 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -31,7 +31,7 @@ using std::placeholders::_1; SmacPlanner2D::SmacPlanner2D() : _a_star(nullptr), - _collision_checker(nullptr, 1), + _collision_checker(nullptr, 1, nullptr), _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) @@ -108,7 +108,7 @@ void SmacPlanner2D::configure( } // Initialize collision checker - _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/); + _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/, node); _collision_checker.setFootprint( costmap_ros->getRobotFootprint(), true /*for 2D, most use radius*/, diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 209d36d39f..2ae0fcb4f4 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2020, Samsung Research America +// Copyright (c) 2023, Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -32,7 +33,7 @@ using std::placeholders::_1; SmacPlannerHybrid::SmacPlannerHybrid() : _a_star(nullptr), - _collision_checker(nullptr, 1), + _collision_checker(nullptr, 1, nullptr), _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) @@ -134,8 +135,8 @@ void SmacPlannerHybrid::configure( node->get_parameter(name + ".lookup_table_size", _lookup_table_size); nav2_util::declare_parameter_if_not_declared( - node, name + ".viz_expansions", rclcpp::ParameterValue(false)); - node->get_parameter(name + ".viz_expansions", _viz_expansions); + node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".debug_visualizations", _debug_visualizations); nav2_util::declare_parameter_if_not_declared( node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); @@ -186,7 +187,7 @@ void SmacPlannerHybrid::configure( } // Initialize collision checker - _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); + _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node); _collision_checker.setFootprint( _costmap_ros->getRobotFootprint(), _costmap_ros->getUseRadius(), @@ -219,8 +220,11 @@ void SmacPlannerHybrid::configure( } _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); - if (_viz_expansions) { + + if (_debug_visualizations) { _expansions_publisher = node->create_publisher("expansions", 1); + _planned_footprints_publisher = node->create_publisher( + "planned_footprints", 1); } RCLCPP_INFO( @@ -238,8 +242,9 @@ void SmacPlannerHybrid::activate() _logger, "Activating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_activate(); - if (_viz_expansions) { + if (_debug_visualizations) { _expansions_publisher->on_activate(); + _planned_footprints_publisher->on_activate(); } if (_costmap_downsampler) { _costmap_downsampler->on_activate(); @@ -256,8 +261,9 @@ void SmacPlannerHybrid::deactivate() _logger, "Deactivating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_deactivate(); - if (_viz_expansions) { + if (_debug_visualizations) { _expansions_publisher->on_deactivate(); + _planned_footprints_publisher->on_activate(); } if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); @@ -278,6 +284,7 @@ void SmacPlannerHybrid::cleanup() } _raw_plan_publisher.reset(); _expansions_publisher.reset(); + _planned_footprints_publisher.reset(); } nav_msgs::msg::Path SmacPlannerHybrid::createPlan( @@ -352,11 +359,14 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( int num_iterations = 0; std::string error; std::unique_ptr>> expansions = nullptr; - if (_viz_expansions) { + if (_debug_visualizations) { expansions = std::make_unique>>(); } // Note: All exceptions thrown are handled by the planner server and returned to the action - if (!_a_star->createPath(path, num_iterations, 0, expansions.get())) { + if (!_a_star->createPath( + path, num_iterations, + _tolerance / static_cast(costmap->getResolution()), expansions.get())) + { if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { @@ -364,8 +374,21 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } } - // Publish expansions for debug - if (_viz_expansions) { + // Convert to world coordinates + plan.poses.reserve(path.size()); + for (int i = path.size() - 1; i >= 0; --i) { + pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); + pose.pose.orientation = getWorldOrientation(path[i].theta); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + _raw_plan_publisher->publish(plan); + } + + if (_debug_visualizations) { + // Publish expansions for debug geometry_msgs::msg::PoseArray msg; geometry_msgs::msg::Pose msg_pose; msg.header.stamp = _clock->now(); @@ -376,19 +399,23 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( 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) { - pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); - pose.pose.orientation = getWorldOrientation(path[i].theta); - plan.poses.push_back(pose); - } + // plot footprint path planned for debug + if (_planned_footprints_publisher->get_subscription_count() > 0) { + auto marker_array = std::make_unique(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now())); + } - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - _raw_plan_publisher->publish(plan); + if (marker_array->markers.empty()) { + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + } + _planned_footprints_publisher->publish(std::move(marker_array)); + } } // Find how much time we have left to do smoothing @@ -552,6 +579,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _lookup_table_dim += 1.0; } + auto node = _node.lock(); + // Re-Initialize A* template if (reinit_a_star) { _a_star = std::make_unique>(_motion_model, _search_info); @@ -567,7 +596,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para // Re-Initialize costmap downsampler if (reinit_downsampler) { if (_downsample_costmap && _downsampling_factor > 1) { - auto node = _node.lock(); std::string topic_name = "downsampled_costmap"; _costmap_downsampler = std::make_unique(); _costmap_downsampler->on_configure( @@ -577,7 +605,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para // Re-Initialize collision checker if (reinit_collision_checker) { - _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); + _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node); _collision_checker.setFootprint( _costmap_ros->getRobotFootprint(), _costmap_ros->getUseRadius(), @@ -586,7 +614,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para // Re-Initialize smoother if (reinit_smoother) { - auto node = _node.lock(); SmootherParams params; params.get(node, _name); _smoother = std::make_unique(params); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 6d00a3479f..8b46fc3ae7 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -31,7 +31,7 @@ using rcl_interfaces::msg::ParameterType; SmacPlannerLattice::SmacPlannerLattice() : _a_star(nullptr), - _collision_checker(nullptr, 1), + _collision_checker(nullptr, 1, nullptr), _smoother(nullptr), _costmap(nullptr) { @@ -169,7 +169,7 @@ void SmacPlannerLattice::configure( // increments causing "wobbly" checks that could cause larger robots to virtually show collisions // in valid configurations. This approximation helps to bound orientation error for all checks // in exchange for slight inaccuracies in the collision headings in terminal search states. - _collision_checker = GridCollisionChecker(_costmap, 72u); + _collision_checker = GridCollisionChecker(_costmap, 72u, node); _collision_checker.setFootprint( costmap_ros->getRobotFootprint(), costmap_ros->getUseRadius(), @@ -284,7 +284,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( std::string error; // Note: All exceptions thrown are handled by the planner server and returned to the action - if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) + { if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index 76befe954c..8039cf9c51 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -1,3 +1,14 @@ +# Test utils +ament_add_gtest(test_utils + test_utils.cpp +) +ament_target_dependencies(test_utils + ${dependencies} +) +target_link_libraries(test_utils + ${library_name} +) + # Test costmap downsampler ament_add_gtest(test_costmap_downsampler test_costmap_downsampler.cpp diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 72a6ba2968..05dfb0796f 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -39,6 +39,7 @@ RclCppFixture g_rclcppfixture; TEST(AStarTest, test_a_star_2d) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; nav2_smac_planner::AStarAlgorithm a_star( nav2_smac_planner::MotionModel::TWOD, info); @@ -62,7 +63,7 @@ TEST(AStarTest, test_a_star_2d) // functional case testing std::unique_ptr checker = - std::make_unique(costmapA, 1); + std::make_unique(costmapA, 1, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); a_star.setCollisionChecker(checker.get()); a_star.setStart(20u, 20u, 0); @@ -122,6 +123,7 @@ TEST(AStarTest, test_a_star_2d) TEST(AStarTest, test_a_star_se2) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -152,7 +154,7 @@ TEST(AStarTest, test_a_star_se2) } std::unique_ptr checker = - std::make_unique(costmapA, size_theta); + std::make_unique(costmapA, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing @@ -178,6 +180,7 @@ TEST(AStarTest, test_a_star_se2) TEST(AStarTest, test_a_star_lattice) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.05; info.non_straight_penalty = 1.05; @@ -213,7 +216,7 @@ TEST(AStarTest, test_a_star_lattice) } std::unique_ptr checker = - std::make_unique(costmapA, size_theta); + std::make_unique(costmapA, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing @@ -225,7 +228,7 @@ TEST(AStarTest, test_a_star_lattice) // check path is the right size and collision free EXPECT_EQ(num_it, 21); - EXPECT_EQ(path.size(), 49u); + EXPECT_GT(path.size(), 47u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -239,6 +242,7 @@ TEST(AStarTest, test_a_star_lattice) TEST(AStarTest, test_se2_single_pose_path) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -263,7 +267,7 @@ TEST(AStarTest, test_se2_single_pose_path) new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, size_theta); + std::make_unique(costmapA, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp index 40e73c82ad..84f2d5b39d 100644 --- a/nav2_smac_planner/test/test_collision_checker.cpp +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -19,11 +19,21 @@ #include "gtest/gtest.h" #include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_util/lifecycle_node.hpp" using namespace nav2_costmap_2d; // NOLINT +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + TEST(collision_footprint, test_basic) { + auto node = std::make_shared("testA"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); geometry_msgs::msg::Point p1; @@ -41,7 +51,7 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(5.0, 5.0, 0.0, false); float cost = collision_checker.getCost(); @@ -51,9 +61,10 @@ TEST(collision_footprint, test_basic) TEST(collision_footprint, test_point_cost) { + auto node = std::make_shared("testB"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); nav2_costmap_2d::Footprint footprint; collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0); @@ -65,9 +76,10 @@ TEST(collision_footprint, test_point_cost) TEST(collision_footprint, test_world_to_map) { + auto node = std::make_shared("testC"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); nav2_costmap_2d::Footprint footprint; collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0); @@ -90,6 +102,7 @@ TEST(collision_footprint, test_world_to_map) TEST(collision_footprint, test_footprint_at_pose_with_movement) { + auto node = std::make_shared("testD"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 254); for (unsigned int i = 40; i <= 60; ++i) { @@ -113,7 +126,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); @@ -132,6 +145,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) TEST(collision_footprint, test_point_and_line_cost) { + auto node = std::make_shared("testE"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( 100, 100, 0.10000, 0, 0.0, 128.0); @@ -153,7 +167,7 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index f3af941c4b..eede15fa84 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -34,9 +34,10 @@ RclCppFixture g_rclcppfixture; TEST(Node2DTest, test_node_2d) { + auto node = std::make_shared("test"); nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(&costmapA, 72); + std::make_unique(&costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test construction @@ -101,6 +102,7 @@ TEST(Node2DTest, test_node_2d) TEST(Node2DTest, test_node_2d_neighbors) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; unsigned int size_x = 10u; unsigned int size_y = 10u; @@ -122,7 +124,7 @@ TEST(Node2DTest, test_node_2d_neighbors) nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(&costmapA, 72); + std::make_unique(&costmapA, 72, lnode); unsigned char cost = static_cast(1); nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1); node->setCost(cost); diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index e304b97e42..16d6278bad 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -35,6 +35,7 @@ RclCppFixture g_rclcppfixture; TEST(NodeHybridTest, test_node_hybrid) { + auto node = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -56,7 +57,7 @@ TEST(NodeHybridTest, test_node_hybrid) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test construction @@ -135,6 +136,7 @@ TEST(NodeHybridTest, test_node_hybrid) TEST(NodeHybridTest, test_obstacle_heuristic) { + auto node = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -169,7 +171,7 @@ TEST(NodeHybridTest, test_obstacle_heuristic) } } std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid testA(0); @@ -245,6 +247,7 @@ TEST(NodeHybridTest, test_node_debin_neighbors) TEST(NodeHybridTest, test_node_reeds_neighbors) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 1.2; info.non_straight_penalty = 1.4; @@ -284,7 +287,7 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(&costmapA, 72); + std::make_unique(&costmapA, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49); std::function neighborGetter = diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 61479e6e1b..16674d9dad 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -22,9 +22,18 @@ #include "nav2_smac_planner/node_lattice.hpp" #include "gtest/gtest.h" #include "ament_index_cpp/get_package_share_directory.hpp" +#include "nav2_util/lifecycle_node.hpp" using json = nlohmann::json; +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + TEST(NodeLatticeTest, parser_test) { std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); @@ -164,6 +173,7 @@ TEST(NodeLatticeTest, test_node_lattice_conversions) TEST(NodeLatticeTest, test_node_lattice) { + auto node = std::make_shared("test"); std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); std::string filePath = pkg_share_dir + @@ -207,7 +217,7 @@ TEST(NodeLatticeTest, test_node_lattice) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test node valid and cost @@ -241,6 +251,7 @@ TEST(NodeLatticeTest, test_node_lattice) TEST(NodeLatticeTest, test_get_neighbors) { + auto lnode = std::make_shared("test"); std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); std::string filePath = pkg_share_dir + @@ -271,7 +282,7 @@ TEST(NodeLatticeTest, test_get_neighbors) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); std::function neighborGetter = @@ -291,6 +302,7 @@ TEST(NodeLatticeTest, test_get_neighbors) TEST(NodeLatticeTest, test_node_lattice_custom_footprint) { + auto lnode = std::make_shared("test"); std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); std::string filePath = pkg_share_dir + @@ -321,7 +333,7 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint) nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D( 40, 40, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmap, 72); + std::make_unique(costmap, 72, lnode); // Make some custom asymmetrical footprint nav2_costmap_2d::Footprint footprint; diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index d7d27f1d20..acf709c282 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -95,7 +95,7 @@ TEST(SmootherTest, test_full_smoother) a_star.initialize( false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); std::unique_ptr checker = - std::make_unique(costmap, size_theta); + std::make_unique(costmap, size_theta, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // Create A* search to smooth diff --git a/nav2_smac_planner/test/test_utils.cpp b/nav2_smac_planner/test/test_utils.cpp new file mode 100644 index 0000000000..8a6b2440ef --- /dev/null +++ b/nav2_smac_planner/test/test_utils.cpp @@ -0,0 +1,149 @@ +// Copyright (c) 2023 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_smac_planner/utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +using namespace nav2_smac_planner; // NOLINT + +TEST(transform_footprint_to_edges, test_basic) +{ + geometry_msgs::msg::Point p1; + p1.x = 1.0; + p1.y = 1.0; + + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = -1.0; + + geometry_msgs::msg::Point p3; + p3.x = -1.0; + p3.y = -1.0; + + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = 1.0; + + std::vector footprint{p1, p2, p3, p4}; + std::vector footprint_edges{p1, p2, p2, p3, p3, p4, p4, p1}; + + // Identity pose + geometry_msgs::msg::Pose pose0; + auto result = transformFootprintToEdges(pose0, footprint); + EXPECT_EQ(result.size(), 8u); + + for (size_t i = 0; i < result.size(); i++) { + auto & p = result[i]; + auto & q = footprint_edges[i]; + EXPECT_EQ(p.x, q.x); + EXPECT_EQ(p.y, q.y); + } +} + +TEST(transform_footprint_to_edges, test_transition_rotation) +{ + geometry_msgs::msg::Point p1; + p1.x = 1.0; + p1.y = 1.0; + + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = -1.0; + + geometry_msgs::msg::Point p3; + p3.x = -1.0; + p3.y = -1.0; + + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = 1.0; + + geometry_msgs::msg::Pose pose0; + pose0.position.x = 1.0; + pose0.position.y = 1.0; + pose0.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI / 4.0); + + std::vector footprint{p1, p2, p3, p4}; + + // q1 + geometry_msgs::msg::Point q1; + q1.x = 0.0 + pose0.position.x; + q1.y = sqrt(2) + pose0.position.y; + + // q2 + geometry_msgs::msg::Point q2; + q2.x = sqrt(2.0) + pose0.position.x; + q2.y = 0.0 + pose0.position.y; + + // q3 + geometry_msgs::msg::Point q3; + q3.x = 0.0 + pose0.position.x; + q3.y = -sqrt(2) + pose0.position.y; + + // q4 + geometry_msgs::msg::Point q4; + q4.x = -sqrt(2.0) + pose0.position.x; + q4.y = 0.0 + pose0.position.y; + + std::vector footprint_edges{q1, q2, q2, q3, q3, q4, q4, q1}; + auto result = transformFootprintToEdges(pose0, footprint); + EXPECT_EQ(result.size(), 8u); + + for (size_t i = 0; i < result.size(); i++) { + auto & p = result[i]; + auto & q = footprint_edges[i]; + EXPECT_NEAR(p.x, q.x, 1e-3); + EXPECT_NEAR(p.y, q.y, 1e-3); + } +} + +TEST(create_marker, test_createMarker) +{ + geometry_msgs::msg::Point p1; + p1.x = 1.0; + p1.y = 1.0; + + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = -1.0; + + geometry_msgs::msg::Point p3; + p3.x = -1.0; + p3.y = -1.0; + + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = 1.0; + std::vector edges{p1, p2, p3, p4}; + + auto marker1 = createMarker(edges, 10u, "test_frame", rclcpp::Time(0.)); + EXPECT_EQ(marker1.header.frame_id, "test_frame"); + EXPECT_EQ(rclcpp::Time(marker1.header.stamp).nanoseconds(), 0); + EXPECT_EQ(marker1.ns, "planned_footprint"); + EXPECT_EQ(marker1.id, 10u); + EXPECT_EQ(marker1.points.size(), 4u); + + edges.clear(); + auto marker2 = createMarker(edges, 8u, "test_frame2", rclcpp::Time(1.0, 0.0)); + EXPECT_EQ(marker2.header.frame_id, "test_frame2"); + EXPECT_EQ(rclcpp::Time(marker2.header.stamp).nanoseconds(), 1e9); + EXPECT_EQ(marker2.id, 8u); + EXPECT_EQ(marker2.points.size(), 0u); +} diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 9a379d316b..a07a324f6b 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.2.1 + 1.2.2 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index d7afe9b55d..08e475de8e 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -158,8 +158,8 @@ class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber DummyFootprintSubscriber( nav2_util::LifecycleNode::SharedPtr node, const std::string & topic_name, - tf2_ros::Buffer & tf_) - : FootprintSubscriber(node, topic_name, tf_) + tf2_ros::Buffer & tf) + : FootprintSubscriber(node, topic_name, tf) { auto footprint = std::make_shared(); footprint->header.frame_id = "base_link"; // global frame = robot frame to avoid tf lookup @@ -223,21 +223,16 @@ class DummySmootherServer : public nav2_smoother::SmootherServer }; // Define a test class to hold the context for the tests - -class SmootherConfigTest : public ::testing::Test -{ -}; - class SmootherTest : public ::testing::Test { -protected: - SmootherTest() {SetUp();} +public: + SmootherTest() {} ~SmootherTest() {} - void SetUp() + void SetUp() override { - node_lifecycle_ = - std::make_shared( + node_ = + std::make_shared( "LifecycleSmootherTestNode", rclcpp::NodeOptions()); smoother_server_ = std::make_shared(); @@ -252,10 +247,10 @@ class SmootherTest : public ::testing::Test smoother_server_->activate(); client_ = rclcpp_action::create_client( - node_lifecycle_->get_node_base_interface(), - node_lifecycle_->get_node_graph_interface(), - node_lifecycle_->get_node_logging_interface(), - node_lifecycle_->get_node_waitables_interface(), "smooth_path"); + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "smooth_path"); std::cout << "Setup complete." << std::endl; } @@ -264,6 +259,9 @@ class SmootherTest : public ::testing::Test smoother_server_->deactivate(); smoother_server_->cleanup(); smoother_server_->shutdown(); + smoother_server_.reset(); + client_.reset(); + node_.reset(); } bool sendGoal( @@ -291,7 +289,7 @@ class SmootherTest : public ::testing::Test auto future_goal = client_->async_send_goal(goal); - if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) != + if (rclcpp::spin_until_future_complete(node_, future_goal) != rclcpp::FutureReturnCode::SUCCESS) { std::cout << "failed sending goal" << std::endl; @@ -315,12 +313,12 @@ class SmootherTest : public ::testing::Test std::cout << "Getting async result..." << std::endl; auto future_result = client_->async_get_result(goal_handle_); std::cout << "Waiting on future..." << std::endl; - rclcpp::spin_until_future_complete(node_lifecycle_, future_result); + rclcpp::spin_until_future_complete(node_, future_result); std::cout << "future received!" << std::endl; return future_result.get(); } - std::shared_ptr node_lifecycle_; + std::shared_ptr node_; std::shared_ptr smoother_server_; std::shared_ptr> client_; std::shared_ptr> goal_handle_; @@ -387,7 +385,7 @@ TEST_F(SmootherTest, testingCollisionCheckDisabled) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) +TEST(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) { auto smoother_server = std::make_shared(); smoother_server->set_parameter( @@ -402,7 +400,7 @@ TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) +TEST(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) { auto smoother_server = std::make_shared(); smoother_server->set_parameter( @@ -417,7 +415,7 @@ TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin) +TEST(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin) { auto smoother_server = std::make_shared(); auto state = smoother_server->configure(); diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index e65b2f928d..352f9f2277 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(nav2_planner REQUIRED) find_package(navigation2) find_package(angles REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) +find_package(pluginlib REQUIRED) nav2_package() @@ -45,6 +46,7 @@ set(dependencies nav2_navfn_planner angles behaviortree_cpp_v3 + pluginlib ) set(local_controller_plugin_lib local_controller) diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 145d771b28..3aacdace39 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.2.1 + 1.2.2 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 2f29df71d1..a0e20937c8 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.2.1 + 1.2.2 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 22306cb545..bcd2d2ae83 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -21,6 +21,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -58,18 +59,6 @@ bool transformPoseInTargetFrame( tf2_ros::Buffer & tf_buffer, const std::string target_frame, const double transform_timeout = 0.1); -/** - * @brief Obtains a transform from source_frame_id at source_time -> - * to target_frame_id at target_time time - * @param source_frame_id Source frame ID to convert from - * @param source_time Source timestamp to convert from - * @param target_frame_id Target frame ID to convert to - * @param target_time Target time to interpolate to - * @param transform_tolerance Transform tolerance - * @param tf_transform Output source->target transform - * @return True if got correct transform, otherwise false - */ - /** * @brief Obtains a transform from source_frame_id -> to target_frame_id * @param source_frame_id Source frame ID to convert from @@ -109,6 +98,13 @@ bool getTransform( const std::shared_ptr tf_buffer, tf2::Transform & tf2_transform); +/** + * @brief Validates a twist message contains no nans or infs + * @param msg Twist message to validate + * @return True if valid, false if contains unactionable values + */ +bool validateTwist(const geometry_msgs::msg::Twist & msg); + } // end namespace nav2_util #endif // NAV2_UTIL__ROBOT_UTILS_HPP_ diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 62ae1a1559..4b1557a95c 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.2.1 + 1.2.2 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 8714f05a99..e6a6e10cf4 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -15,6 +15,7 @@ // limitations under the License. #include +#include #include #include "tf2/convert.h" @@ -142,4 +143,33 @@ bool getTransform( return true; } +bool validateTwist(const geometry_msgs::msg::Twist & msg) +{ + if (std::isinf(msg.linear.x) || std::isnan(msg.linear.x)) { + return false; + } + + if (std::isinf(msg.linear.y) || std::isnan(msg.linear.y)) { + return false; + } + + if (std::isinf(msg.linear.z) || std::isnan(msg.linear.z)) { + return false; + } + + if (std::isinf(msg.angular.x) || std::isnan(msg.angular.x)) { + return false; + } + + if (std::isinf(msg.angular.y) || std::isnan(msg.angular.y)) { + return false; + } + + if (std::isinf(msg.angular.z) || std::isnan(msg.angular.z)) { + return false; + } + + return true; +} + } // end namespace nav2_util diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp index bd277515ab..d8922868fb 100644 --- a/nav2_util/test/test_robot_utils.cpp +++ b/nav2_util/test/test_robot_utils.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_util/robot_utils.hpp" #include "tf2_ros/transform_listener.h" @@ -32,3 +33,28 @@ TEST(RobotUtils, LookupExceptionError) global_pose.header.frame_id = "base_link"; ASSERT_FALSE(nav2_util::transformPoseInTargetFrame(global_pose, global_pose, tf, "map", 0.1)); } + +TEST(RobotUtils, validateTwist) +{ + geometry_msgs::msg::Twist msg; + EXPECT_TRUE(nav2_util::validateTwist(msg)); + + msg.linear.x = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.linear.x = 1; + msg.linear.y = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.linear.y = 1; + msg.linear.z = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + + msg.linear.z = 1; + msg.angular.x = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.angular.x = 1; + msg.angular.y = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.angular.y = 1; + msg.angular.z = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); +} diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp index c3e7835c6b..a059e74076 100644 --- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp +++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp @@ -25,6 +25,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/odometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_velocity_smoother { diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 0a70ffc3a4..b1b1ab2807 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.2.1 + 1.2.2 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index aa5fbec783..bc101fd732 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -74,11 +74,18 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) node->get_parameter("max_accel", max_accels_); node->get_parameter("max_decel", max_decels_); - for (unsigned int i = 0; i != max_decels_.size(); i++) { + for (unsigned int i = 0; i != 3; i++) { if (max_decels_[i] > 0.0) { - RCLCPP_WARN( - get_logger(), - "Positive values set of deceleration! These should be negative to slow down!"); + throw std::runtime_error( + "Positive values set of deceleration! These should be negative to slow down!"); + } + if (max_accels_[i] < 0.0) { + throw std::runtime_error( + "Negative values set of acceleration! These should be positive to speed up!"); + } + if (min_velocities_[i] > max_velocities_[i]) { + throw std::runtime_error( + "Min velocities are higher than max velocities!"); } } @@ -174,6 +181,12 @@ VelocitySmoother::on_shutdown(const rclcpp_lifecycle::State &) void VelocitySmoother::inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { + // If message contains NaN or Inf, ignore + if (!nav2_util::validateTwist(*msg)) { + RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!"); + return; + } + command_ = msg; last_command_time_ = now(); } @@ -352,8 +365,24 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } else if (name == "min_velocity") { min_velocities_ = parameter.as_double_array(); } else if (name == "max_accel") { + for (unsigned int i = 0; i != 3; i++) { + if (parameter.as_double_array()[i] < 0.0) { + RCLCPP_WARN( + get_logger(), + "Negative values set of acceleration! These should be positive to speed up!"); + result.successful = false; + } + } max_accels_ = parameter.as_double_array(); } else if (name == "max_decel") { + for (unsigned int i = 0; i != 3; i++) { + if (parameter.as_double_array()[i] > 0.0) { + RCLCPP_WARN( + get_logger(), + "Positive values set of deceleration! These should be negative to slow down!"); + result.successful = false; + } + } max_decels_ = parameter.as_double_array(); } else if (name == "deadband_velocity") { deadband_velocities_ = parameter.as_double_array(); diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index a1b2f5454a..c58e20b22c 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -593,6 +593,28 @@ TEST(VelocitySmootherTest, testInvalidParams) EXPECT_THROW(smoother->configure(state), std::runtime_error); } +TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) +{ + auto smoother = + std::make_shared(); + + std::vector bad_test_accel{-10.0, -10.0, -10.0}; + std::vector bad_test_decel{10.0, 10.0, 10.0}; + std::vector bad_test_min_vel{10.0, 10.0, 10.0}; + std::vector bad_test_max_vel{-10.0, -10.0, -10.0}; + + smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel)); + smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel)); + rclcpp_lifecycle::State state; + EXPECT_THROW(smoother->configure(state), std::runtime_error); + + smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel))); + EXPECT_THROW(smoother->configure(state), std::runtime_error); + + smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel))); + EXPECT_THROW(smoother->configure(state), std::runtime_error); +} + TEST(VelocitySmootherTest, testDynamicParameter) { auto smoother = @@ -613,6 +635,8 @@ TEST(VelocitySmootherTest, testDynamicParameter) std::vector min_accel{0.0, 0.0, 0.0}; std::vector deadband{0.0, 0.0, 0.0}; std::vector bad_test{0.0, 0.0}; + std::vector bad_test_accel{-10.0, -10.0, -10.0}; + std::vector bad_test_decel{10.0, 10.0, 10.0}; auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter("smoothing_frequency", 100.0), @@ -662,6 +686,16 @@ TEST(VelocitySmootherTest, testDynamicParameter) rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); EXPECT_FALSE(results.get().successful); + // Test invalid accel / decel + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("max_accel", bad_test_accel)}); + rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); + EXPECT_FALSE(results.get().successful); + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("max_decel", bad_test_decel)}); + rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); + EXPECT_FALSE(results.get().successful); + // test full state after major changes smoother->deactivate(state); smoother->cleanup(state); diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index b65aa682a0..f3952abefb 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.2.1 + 1.2.2 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index a594cb69da..25568c488d 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.2.1 + 1.2.2 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index f035f50f4f..2908a8554f 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.1 + 1.2.2 ROS2 Navigation Stack