From 4cb6aba4ac9b98caa78cd0e91539b6e9f8f85818 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 23 May 2024 16:34:28 -0700 Subject: [PATCH] Iron Sync 6: May 23, 2024 (#4366) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Scale cost critic's weight when dynamically updated (#4246) * Scale cost critic's weight when dynamically updated Signed-off-by: pepisg * sign off Signed-off-by: pepisg --------- Signed-off-by: pepisg * Add expanding the ~/ to the full home dir of user in the path to the map yaml. (#4258) * Add user home expander of home sequence Signed-off-by: Wiktor Bajor * Add passing home dir as string instead of const char* Signed-off-by: Wiktor Bajor * Add docs Signed-off-by: Wiktor Bajor * Fix function declaration Signed-off-by: Wiktor Bajor * Fix linter issues Signed-off-by: Wiktor Bajor * Uncrustify linter Signed-off-by: Wiktor Bajor * Uncrustify linter Signed-off-by: Wiktor Bajor * Uncrustify linter: remove remove whitespace Signed-off-by: Wiktor Bajor --------- Signed-off-by: Wiktor Bajor * Implement Critic for Velocity Deadband Hardware Constraints (#4256) * Adding new velocity deadband critic. - add some tests - cast double to float - add new features from "main" branch - fix formating - add cost test - fix linting issue - add README Signed-off-by: Denis Sokolov * Remove velocity deadband critic from defaults Signed-off-by: Denis Sokolov * remove old weight Signed-off-by: Denis Sokolov * fix velocity deadband critic tests Signed-off-by: Denis Sokolov --------- Signed-off-by: Denis Sokolov * removing clearable layer param (unused) (#4280) * provide message validation check API (#4276) * provide validation_message.hpp Signed-off-by: goes * fix typo Signed-off-by: goes * add test_validation_messages.cpp Signed-off-by: goes * change include-order Signed-off-by: goes * reformat Signed-off-by: goes * update test Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes * [RotationShimController] rotate also on short paths (#4290) When the path is shorter than ´forward_sampling_distance´ the rotatitonShimController would directly give control to the primary controller to navigate to the goal. This would lead to the exact behavior that was tried to be fixed by the rotationShimController: "The result is an awkward, stuttering, or whipping around behavior" [1]. It often resulted in navigation failure. In this case, the controller should try to rotate towards the last point of the path, so that the primary controller can have a better starting orientation. [1] https://navigation.ros.org/tuning/index.html#rotate-in-place-behavior * Add footprint clearing for static layer (#4282) * Add footprint clearing for static layer Signed-off-by: Tony Najjar * fix flckering --------- Signed-off-by: Tony Najjar * Fix #4268 (#4296) Signed-off-by: Steve Macenski * Fix undefined symbols in `libpf_lib.so` (#4312) When I build `nav2_amcl` with `-Wl,--no-undefined` I noticed `libpf_lib.so` has undefined symbols. This PR correctly links `libpf_lib.so` to `libm` so all symbols can be found. You can verify this by executing the following command: ``` ldd -r ./build/nav2_amcl/src/pf/libpf_lib.so linux-vdso.so.1 (0x00007ffd1f8c0000) libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x000074e909a00000) /lib64/ld-linux-x86-64.so.2 (0x000074e909e60000) undefined symbol: ceil (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: atan2 (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: sin (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: hypot (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: cos (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: log (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: sqrt (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: floor (./build/nav2_amcl/src/pf/libpf_lib.so) ``` Signed-off-by: Ramon Wijnands * msg validation check for `/initialpose` in `nav2_amcl` (#4301) * add validation check for PoseWithCovarianceStamped Signed-off-by: goes * remove rebundant check before Signed-off-by: goes * reformat Signed-off-by: goes * typo fixed Signed-off-by: goes * change the type-name Signed-off-by: goes * update test Signed-off-by: goes * reformat Signed-off-by: goes * . Signed-off-by: goes * add comment Signed-off-by: goes * update comment Signed-off-by: goes * change header Signed-off-by: goes * update test Signed-off-by: goes * typo fixed Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes * 4320: Changed precision of calculations of the HybridNode MotionTable::getClosestAngularBin. (#4324) Signed-off-by: Krzysztof Pawełczyk Co-authored-by: Krzysztof Pawełczyk * [LifecycleNode] add bond_heartbeat_period (#4342) * add bond_heartbeat_period Signed-off-by: Guillaume Doisy * lint Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * Update path_longer_on_approach.cpp (#4344) Fix the bug that isPathUpdated function will return false for the reason that it compare the timestaped between new path and old path's last pose Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> * [LifecycleManagerClient] clean set_initial_pose and navigate_to_pose (#4346) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * Move projectState after getPointsInside (#4356) * Modify test to check fix Signed-off-by: Brice * Add static polygon check before simulation Signed-off-by: Brice --------- Signed-off-by: Brice * adding final pose in analytic expansion to check (#4353) * bump iron to 1.2.8 * fix merge conflict resolution --------- Signed-off-by: pepisg Signed-off-by: Wiktor Bajor Signed-off-by: Denis Sokolov Signed-off-by: goes Signed-off-by: Tony Najjar Signed-off-by: Steve Macenski Signed-off-by: Ramon Wijnands Signed-off-by: Krzysztof Pawełczyk Signed-off-by: Guillaume Doisy Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> Signed-off-by: Brice Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Co-authored-by: Sokolov Denis <52282102+perchess@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: goes Co-authored-by: Saitama Co-authored-by: Tony Najjar Co-authored-by: Ramon Wijnands Co-authored-by: AzaelCicero Co-authored-by: Krzysztof Pawełczyk Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: StetroF <120172218+StetroF@users.noreply.github.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> --- nav2_amcl/package.xml | 2 +- nav2_amcl/src/amcl_node.cpp | 12 +- nav2_amcl/src/pf/CMakeLists.txt | 1 + nav2_behavior_tree/package.xml | 2 +- .../decorator/path_longer_on_approach.cpp | 2 +- nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- nav2_collision_monitor/src/polygon.cpp | 7 +- .../test/collision_monitor_node_test.cpp | 3 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- .../nav2_costmap_2d/clear_costmap_service.hpp | 1 - .../include/nav2_costmap_2d/static_layer.hpp | 12 + nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/plugins/static_layer.cpp | 35 +- nav2_costmap_2d/src/clear_costmap_service.cpp | 2 - nav2_costmap_2d/src/costmap_2d_ros.cpp | 3 - nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- 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 +- .../lifecycle_manager_client.hpp | 17 - nav2_lifecycle_manager/package.xml | 2 +- .../include/nav2_map_server/map_io.hpp | 11 + nav2_map_server/package.xml | 2 +- nav2_map_server/src/map_io.cpp | 28 +- nav2_map_server/test/unit/test_map_io.cpp | 27 ++ nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/README.md | 14 + nav2_mppi_controller/critics.xml | 4 + .../critics/velocity_deadband_critic.hpp | 54 +++ nav2_mppi_controller/package.xml | 2 +- .../src/critics/cost_critic.cpp | 6 + .../src/critics/velocity_deadband_critic.cpp | 104 +++++ nav2_mppi_controller/test/critics_tests.cpp | 92 +---- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- .../package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- .../src/nav2_rotation_shim_controller.cpp | 5 +- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smac_planner/src/analytic_expansion.cpp | 2 +- nav2_smac_planner/src/collision_checker.cpp | 2 +- nav2_smac_planner/src/node_hybrid.cpp | 3 +- nav2_smac_planner/test/test_nodehybrid.cpp | 15 +- nav2_smoother/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- .../include/nav2_util/lifecycle_node.hpp | 1 + .../include/nav2_util/validate_messages.hpp | 179 +++++++++ nav2_util/package.xml | 2 +- nav2_util/src/lifecycle_node.cpp | 31 +- nav2_util/test/CMakeLists.txt | 4 + nav2_util/test/test_validation_messages.cpp | 368 ++++++++++++++++++ nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 69 files changed, 958 insertions(+), 164 deletions(-) create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp create mode 100644 nav2_util/include/nav2_util/validate_messages.hpp create mode 100644 nav2_util/test/test_validation_messages.cpp diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 93c4560982..27d1e917c7 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.2.7 + 1.2.8

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 87b9559223..6e92626fc9 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -49,6 +49,7 @@ #pragma GCC diagnostic pop #include "nav2_amcl/portable_utils.hpp" +#include "nav2_util/validate_messages.hpp" using namespace std::placeholders; using rcl_interfaces::msg::ParameterType; @@ -523,11 +524,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha RCLCPP_INFO(get_logger(), "initialPoseReceived"); - if (msg->header.frame_id == "") { - // This should be removed at some point - RCLCPP_WARN( - get_logger(), - "Received initial pose with empty frame_id. You should always supply a frame_id."); + if (!nav2_util::validateMsg(*msg)) { + RCLCPP_ERROR(get_logger(), "Received initialpose message is malformed. Rejecting."); return; } if (nav2_util::strip_leading_slash(msg->header.frame_id) != global_frame_id_) { @@ -1390,6 +1388,10 @@ void AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { RCLCPP_DEBUG(get_logger(), "AmclNode: A new map was received."); + if (!nav2_util::validateMsg(*msg)) { + RCLCPP_ERROR(get_logger(), "Received map message is malformed. Rejecting."); + return; + } if (first_map_only_ && first_map_received_) { return; } diff --git a/nav2_amcl/src/pf/CMakeLists.txt b/nav2_amcl/src/pf/CMakeLists.txt index c6f16e6a7d..3b4b2fa5ca 100644 --- a/nav2_amcl/src/pf/CMakeLists.txt +++ b/nav2_amcl/src/pf/CMakeLists.txt @@ -15,6 +15,7 @@ target_include_directories(pf_lib PRIVATE ../include) if(HAVE_DRAND48) target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48") endif() +target_link_libraries(pf_lib m) install(TARGETS pf_lib diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 53ca2b0dca..4354d1fc94 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.2.7 + 1.2.8 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index d4b40b0964..b8c5bdbb46 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -36,7 +36,7 @@ bool PathLongerOnApproach::isPathUpdated( { return new_path != old_path && old_path.poses.size() != 0 && new_path.poses.size() != 0 && - old_path.poses.back() == new_path.poses.back(); + old_path.poses.back().pose == new_path.poses.back().pose; } bool PathLongerOnApproach::isRobotInGoalProximity( diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 6ab1b283d2..51b422dbc6 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.2.7 + 1.2.8 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 208e688218..d9ac165a60 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.7 + 1.2.8 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 f4a3dbe655..308a0ff0ad 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.2.7 + 1.2.8 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index c0ea7fea45..9c493ffac6 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.2.7 + 1.2.8 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index c215cdf2da..416e3e1cd5 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -248,7 +248,12 @@ double Polygon::getCollisionTime( Velocity vel = velocity; // Array of points transformed to the frame concerned with pose on each simulation step - std::vector points_transformed; + std::vector points_transformed = collision_points; + + // Check static polygon + if (getPointsInside(points_transformed) >= min_points_) { + return 0.0; + } // Robot movement simulation for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) { diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 2ceb34fff6..baee9e4903 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -792,7 +792,8 @@ TEST_F(Tester, testProcessApproach) // 3. Obstacle is inside robot footprint publishScan(0.5, curr_time); ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); - publishCmdVel(0.5, 0.2, 0.0); + // Publish impossible cmd_vel to ensure robot footprint is checked + publishCmdVel(1000000000.0, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 74a7e69a3f..19aa6d0bd1 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.2.7 + 1.2.8 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index c90edc7c82..8c9c87484c 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.2.7 + 1.2.8 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 48a7359614..2d9416db32 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.2.7 + 1.2.8 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 37e4a6a4c1..535be634c6 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.2.7 + 1.2.8 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/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 46d79bd9df..2d74530b43 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp @@ -67,7 +67,6 @@ class ClearCostmapService // Clearing parameters unsigned char reset_value_; - std::vector clearable_layers_; // Server for clearing the costmap rclcpp::Service::SharedPtr clear_except_service_; 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 dd1a65cef0..967a6c3789 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -48,6 +48,7 @@ #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/footprint.hpp" namespace nav2_costmap_2d { @@ -160,6 +161,17 @@ class StaticLayer : public CostmapLayer rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + std::vector transformed_footprint_; + bool footprint_clearing_enabled_; + /** + * @brief Clear costmap layer info below the robot's footprint + */ + void updateFootprint( + double robot_x, double robot_y, double robot_yaw, double * min_x, + double * min_y, + double * max_x, + double * max_y); + std::string global_frame_; ///< @brief The global frame for the costmap std::string map_frame_; /// @brief frame that map is located in diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index f13d8c0c5c..a49554cc44 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.2.7 + 1.2.8 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/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 18cd51164f..729530d82b 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -45,6 +45,7 @@ #include "pluginlib/class_list_macros.hpp" #include "tf2/convert.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_util/validate_messages.hpp" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) @@ -132,6 +133,7 @@ StaticLayer::getParameters() declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); declareParameter("map_topic", rclcpp::ParameterValue("")); + declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false)); auto node = node_.lock(); if (!node) { @@ -140,6 +142,7 @@ StaticLayer::getParameters() node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); + node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); std::string private_map_topic, global_map_topic; node->get_parameter(name_ + "." + "map_topic", private_map_topic); node->get_parameter("map_topic", global_map_topic); @@ -277,6 +280,10 @@ StaticLayer::interpretValue(unsigned char value) void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { + if (!nav2_util::validateMsg(*new_map)) { + RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting."); + return; + } if (!map_received_) { processMap(*new_map); map_received_ = true; @@ -328,7 +335,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u void StaticLayer::updateBounds( - double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x, + double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) @@ -366,6 +373,24 @@ StaticLayer::updateBounds( *max_y = std::max(wy, *max_y); has_updated_data_ = false; + + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void +StaticLayer::updateFootprint( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, + double * max_x, + double * max_y) +{ + if (!footprint_clearing_enabled_) {return;} + + transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + + for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { + touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); + } } void @@ -387,6 +412,10 @@ StaticLayer::updateCosts( return; } + if (footprint_clearing_enabled_) { + setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE); + } + if (!layered_costmap_->isRolling()) { // if not rolling, the layered costmap (master_grid) has same coordinates as this layer if (!use_maximum_) { @@ -469,6 +498,10 @@ StaticLayer::dynamicParametersCallback( has_updated_data_ = true; current_ = false; } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "footprint_clearing_enabled") { + footprint_clearing_enabled_ = parameter.as_bool(); + } } } result.successful = true; diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 18039f4298..857278b841 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -40,8 +40,6 @@ ClearCostmapService::ClearCostmapService( logger_ = node->get_logger(); reset_value_ = costmap_.getCostmap()->getDefaultValue(); - node->get_parameter("clearable_layers", clearable_layers_); - clear_except_service_ = node->create_service( "clear_except_" + costmap_.getName(), std::bind( diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 8bcd39683c..8934ee4422 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -108,8 +108,6 @@ void Costmap2DROS::init() { RCLCPP_INFO(get_logger(), "Creating Costmap"); - std::vector clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"}; - declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false)); declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f)); declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]"))); @@ -134,7 +132,6 @@ void Costmap2DROS::init() declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); declare_parameter("use_maximum", rclcpp::ParameterValue(false)); - declare_parameter("clearable_layers", rclcpp::ParameterValue(clearable_layers)); } Costmap2DROS::~Costmap2DROS() diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index b03c8096aa..0708195431 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.7 + 1.2.8 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 279f4264f4..0c1d9c2257 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.7 + 1.2.8 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index f3b7abf481..ad4b5f0fec 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.7 + 1.2.8 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 0a596b77be..9ada111110 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.7 + 1.2.8 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 3526def3f2..b0b9eb77b1 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.7 + 1.2.8 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 8b5c02d4f7..b7005a361a 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.7 + 1.2.8 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 1ed2f94a32..15c5520368 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.7 + 1.2.8 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 0b9425c70b..ff94df6cb3 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.7 + 1.2.8 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index c5f2be8bb6..076e848902 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -84,23 +84,6 @@ class LifecycleManagerClient */ SystemStatus is_active(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - // A couple convenience methods to facilitate scripting tests - /** - * @brief Set initial pose with covariance - * @param x X position - * @param y Y position - * @param theta orientation - */ - void set_initial_pose(double x, double y, double theta); - /** - * @brief Send goal pose to NavigationToPose action server - * @param x X position - * @param y Y position - * @param theta orientation - * @return true or false - */ - bool navigate_to_pose(double x, double y, double theta); - protected: using ManageLifecycleNodes = nav2_msgs::srv::ManageLifecycleNodes; diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 93062f985c..e222d526e8 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.2.7 + 1.2.8 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/include/nav2_map_server/map_io.hpp b/nav2_map_server/include/nav2_map_server/map_io.hpp index bee7df7313..97dc81821c 100644 --- a/nav2_map_server/include/nav2_map_server/map_io.hpp +++ b/nav2_map_server/include/nav2_map_server/map_io.hpp @@ -98,6 +98,17 @@ bool saveMapToFile( const nav_msgs::msg::OccupancyGrid & map, const SaveParameters & save_parameters); +/** + * @brief Expand ~/ to home user dir. + * @param yaml_filename Name of input YAML file. + * @param home_dir Expanded `~/`home dir or empty string if HOME not set + * + * @return Expanded string or input string if `~/` not expanded + */ +std::string expand_user_home_dir_if_needed( + std::string yaml_filename, + std::string home_dir); + } // namespace nav2_map_server #endif // NAV2_MAP_SERVER__MAP_IO_HPP_ diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 24f5315d0c..98e08c8b80 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.2.7 + 1.2.8 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 4803b6d3f6..85428490ed 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include "Magick++.h" #include "nav2_util/geometry_utils.hpp" @@ -115,9 +116,33 @@ T yaml_get_value(const YAML::Node & node, const std::string & key) } } +std::string get_home_dir() +{ + if (const char * home_dir = std::getenv("HOME")) { + return std::string{home_dir}; + } + return std::string{}; +} + +std::string expand_user_home_dir_if_needed( + std::string yaml_filename, + std::string home_variable_value) +{ + if (yaml_filename.size() < 2 || !(yaml_filename[0] == '~' && yaml_filename[1] == '/')) { + return yaml_filename; + } + if (home_variable_value.empty()) { + std::cout << "[INFO] [map_io]: Map yaml file name starts with '~/' but no HOME variable set. \n" + << "[INFO] [map_io] User home dir will be not expanded \n"; + return yaml_filename; + } + const std::string prefix{home_variable_value}; + return yaml_filename.replace(0, 1, prefix); +} + LoadParameters loadMapYaml(const std::string & yaml_filename) { - YAML::Node doc = YAML::LoadFile(yaml_filename); + YAML::Node doc = YAML::LoadFile(expand_user_home_dir_if_needed(yaml_filename, get_home_dir())); LoadParameters load_parameters; auto image_file_name = yaml_get_value(doc, "image"); @@ -295,7 +320,6 @@ LOAD_MAP_STATUS loadMapFromYaml( " for reason: " << e.what() << std::endl; return INVALID_MAP_METADATA; } - try { loadMapFromFile(load_parameters, map); } catch (std::exception & e) { diff --git a/nav2_map_server/test/unit/test_map_io.cpp b/nav2_map_server/test/unit/test_map_io.cpp index 419ce669ef..aed2a2dde3 100644 --- a/nav2_map_server/test/unit/test_map_io.cpp +++ b/nav2_map_server/test/unit/test_map_io.cpp @@ -311,3 +311,30 @@ TEST_F(MapIOTester, loadInvalidYAML) LoadParameters loadParameters; ASSERT_ANY_THROW(loadParameters = loadMapYaml(path(TEST_DIR) / path("invalid_file.yaml"))); } + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenShorterThanTwo) +{ + const std::string emptyFileName{}; + ASSERT_EQ(emptyFileName, expand_user_home_dir_if_needed(emptyFileName, "/home/user")); +} + +TEST( + HomeUserExpanderTestSuite, + homeUserExpanderShouldNotChangeInputStringWhenInputStringDoesNotStartWithHomeSequence) +{ + const std::string fileName{"valid_file.yaml"}; + ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, "/home/user")); +} + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenHomeVariableNotFound) +{ + const std::string fileName{"~/valid_file.yaml"}; + ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, "")); +} + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldExpandHomeSequenceWhenHomeVariableSet) +{ + const std::string fileName{"~/valid_file.yaml"}; + const std::string expectedOutputFileName{"/home/user/valid_file.yaml"}; + ASSERT_EQ(expectedOutputFileName, expand_user_home_dir_if_needed(fileName, "/home/user")); +} diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index d2bb6d9c9d..3ca6735e40 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -89,6 +89,7 @@ add_library(mppi_critics SHARED src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp src/critics/constraint_critic.cpp + src/critics/velocity_deadband_critic.cpp ) set(libraries mppi_controller mppi_critics) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 740dcae1b6..f8a90f1cbc 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -172,6 +172,15 @@ Note: There is a "Legacy" version of this critic also available with the same pa | cost_weight | double | Default 10.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | + +#### Velocity Deadband Critic + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | Default 35.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | deadband_velocities | double[] | Default [0.0, 0.0, 0.0]. The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action. | + + ### XML configuration example ``` controller_server: @@ -262,6 +271,11 @@ controller_server: threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 forward_preference: true + # VelocityDeadbandCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 35.0 + # deadband_velocities: [0.05, 0.05, 0.05] # TwirlingCritic: # enabled: true # twirling_cost_power: 1 diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index d578d23a9e..6085dbec88 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -45,5 +45,9 @@ mppi critic for incentivizing moving within kinematic and dynamic bounds + + mppi critic for restricting command velocities in deadband range + + diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp new file mode 100644 index 0000000000..76e1dbd670 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp @@ -0,0 +1,54 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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. + +#ifndef NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ + +#include + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::VelocityDeadbandCritic + * @brief Critic objective function for enforcing feasible constraints + */ +class VelocityDeadbandCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to goal following + * + * @param costs [out] add reference cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + unsigned int power_{0}; + float weight_{0}; + std::vector deadband_velocities_{0.0f, 0.0f, 0.0f}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index dd6142c52f..ff0683cd40 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.2.7 + 1.2.8 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index b91ab075b6..a9dd9e9e81 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -33,6 +33,12 @@ void CostCritic::initialize() // Normalized by cost value to put in same regime as other weights weight_ /= 254.0f; + // Normalize weight when parameter is changed dynamically as well + auto weightDynamicCb = [&](const rclcpp::Parameter & weight) { + weight_ = weight.as_double() / 254.0f; + }; + parameters_handler_->addDynamicParamCallback(name_ + ".cost_weight", weightDynamicCb); + collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); diff --git a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp new file mode 100644 index 0000000000..84d3aba303 --- /dev/null +++ b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "nav2_mppi_controller/critics/velocity_deadband_critic.hpp" + +namespace mppi::critics +{ + +void VelocityDeadbandCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 35.0); + + // Recast double to float + std::vector deadband_velocities{0.0, 0.0, 0.0}; + getParam(deadband_velocities, "deadband_velocities", std::vector{0.0, 0.0, 0.0}); + std::transform( + deadband_velocities.begin(), deadband_velocities.end(), deadband_velocities_.begin(), + [](double d) {return static_cast(d);}); + + RCLCPP_INFO_STREAM( + logger_, "VelocityDeadbandCritic instantiated with " + << power_ << " power, " << weight_ << " weight, deadband_velocity [" + << deadband_velocities_.at(0) << "," << deadband_velocities_.at(1) << "," + << deadband_velocities_.at(2) << "]"); +} + +void VelocityDeadbandCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + + if (!enabled_) { + return; + } + + auto & vx = data.state.vx; + auto & wz = data.state.wz; + + if (data.motion_model->isHolonomic()) { + auto & vy = data.state.vy; + if (power_ > 1u) { + data.costs += xt::pow( + xt::sum( + std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * + data.model_dt, + {1}, immediate) * + weight_, + power_); + } else { + data.costs += xt::sum( + (std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * + data.model_dt, + {1}, immediate) * + weight_; + } + return; + } + + if (power_ > 1u) { + data.costs += xt::pow( + xt::sum( + std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * + data.model_dt, + {1}, immediate) * + weight_, + power_); + } else { + data.costs += xt::sum( + (std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * + data.model_dt, + {1}, immediate) * + weight_; + } + return; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS(mppi::critics::VelocityDeadbandCritic, mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index af2a61e20e..eb2a79fab7 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -31,6 +31,7 @@ #include "nav2_mppi_controller/critics/path_follow_critic.hpp" #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp" #include "nav2_mppi_controller/critics/twirling_critic.hpp" +#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp" #include "utils_test.cpp" // NOLINT // Tests the various critic plugin functions @@ -609,106 +610,51 @@ TEST(CriticTests, PathAlignCritic) EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); } -TEST(CriticTests, PathAlignLegacyCritic) +TEST(CriticTests, VelocityDeadbandCritic) { // Standard preamble auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", "dummy_costmap", true); ParametersHandler param_handler(node); + auto getParam = param_handler.getParamGetter("critic"); + std::vector deadband_velocities_; + getParam(deadband_velocities_, "deadband_velocities", std::vector{0.08, 0.08, 0.08}); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; - generated_trajectories.reset(1000, 30); models::Path path; xt::xtensor costs = xt::zeros({1000}); float model_dt = 0.1; CriticData data = {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; - data.motion_model = std::make_shared(); - TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally - data.goal_checker = &goal_checker; + data.motion_model = std::make_shared(); // Initialization testing - // Make sure initializes correctly - PathAlignLegacyCritic critic; + // Make sure initializes correctly and that defaults are reasonable + VelocityDeadbandCritic critic; critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); EXPECT_EQ(critic.getName(), "critic"); // Scoring testing - // provide state poses and path close within positional tolerances - state.pose.pose.position.x = 1.0; - path.reset(10); - path.x(9) = 0.85; - critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); - - // provide state pose and path far enough to enable - // but data furthest point reached is 0 and offset default is 20, so returns - path.x(9) = 0.15; - critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); - - // provide state pose and path far enough to enable, with data to pass condition - // but with empty trajectories and paths, should still be zero - *data.furthest_reached_path_point = 21; - path.x(9) = 0.15; + // provide velocities out of deadband bounds, should not have any costs + state.vx = 0.80 * xt::ones({1000, 30}); + state.vy = 0.60 * xt::ones({1000, 30}); + state.wz = 0.80 * xt::ones({1000, 30}); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); - - // provide state pose and path far enough to enable, with data to pass condition - // and with a valid path to pass invalid path condition - state.pose.pose.position.x = 0.0; - data.path_pts_valid.reset(); // Recompute on new path - path.reset(22); - path.x(0) = 0; - path.x(1) = 0.1; - path.x(2) = 0.2; - path.x(3) = 0.3; - path.x(4) = 0.4; - path.x(5) = 0.5; - path.x(6) = 0.6; - path.x(7) = 0.7; - path.x(8) = 0.8; - path.x(9) = 0.9; - path.x(10) = 0.9; - path.x(11) = 0.9; - path.x(12) = 0.9; - path.x(13) = 0.9; - path.x(14) = 0.9; - path.x(15) = 0.9; - path.x(16) = 0.9; - path.x(17) = 0.9; - path.x(18) = 0.9; - path.x(19) = 0.9; - path.x(20) = 0.9; - path.x(21) = 0.9; - generated_trajectories.x = 0.66 * xt::ones({1000, 30}); - critic.score(data); - // 0.04 * 1000 * 10 weight * 6 num pts eval / 6 normalization term - EXPECT_NEAR(xt::sum(costs, immediate)(), 400.0, 1e-2); - - // provide state pose and path far enough to enable, with data to pass condition - // but path is blocked in collision - auto * costmap = costmap_ros->getCostmap(); - // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution - for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m - for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m - costmap->setCost(i, j, 254); - } - } + EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); - data.path_pts_valid.reset(); // Recompute on new path - costs = xt::zeros({1000}); - path.x = 1.5 * xt::ones({22}); - path.y = 1.5 * xt::ones({22}); + // Test cost value + state.vx = 0.01 * xt::ones({1000, 30}); + state.vy = 0.02 * xt::ones({1000, 30}); + state.wz = 0.021 * xt::ones({1000, 30}); critic.score(data); - EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 + EXPECT_NEAR(costs(1), 19.845, 0.01); } diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 3261672ed7..2bfd8b716f 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.2.7 + 1.2.8 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 97fe29232b..70f129d0ad 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.2.7 + 1.2.8 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 58b8343666..667dac75c5 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.2.7 + 1.2.8 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 72b5c09665..047e5dc902 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.7 + 1.2.8 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 97989ee983..cd9a540084 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.7 + 1.2.8 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index afd3238145..5097a1fc9c 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -195,10 +195,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - throw nav2_core::ControllerException( - std::string( - "Unable to find a sampling point at least %0.2f from the robot," - "passing off to primary controller plugin.", forward_sampling_distance_)); + return current_path_.poses.back(); } geometry_msgs::msg::Pose diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 659058f84f..5512f763dd 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.2.7 + 1.2.8 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index d6557a2c7a..e6f908ce3d 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.2.7 + 1.2.8 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index a5406bc37b..e8c3d43625 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.2.7 + 1.2.8 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 831bccbd9e..d0e068c634 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -161,7 +161,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionmotion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); reals = s.reals(); // Make sure in range [0, 2PI) diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 549570fb26..59e7f6760b 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -97,7 +97,7 @@ bool GridCollisionChecker::inCollision( if (outsideRange(costmap_->getSizeInCellsX(), x) || outsideRange(costmap_->getSizeInCellsY(), y)) { - return false; + return true; } // Assumes setFootprint already set diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 0e86e7c570..304578de3d 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -250,7 +250,8 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) { - return static_cast(floor(static_cast(theta) / bin_size)); + return static_cast(floor(theta / static_cast(bin_size))) % + num_angle_quantization; } float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 5580e64d04..0050b0a0ab 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -13,6 +13,7 @@ // limitations under the License. Reserved. #include +#include #include #include #include @@ -312,6 +313,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = 3.1415926; + motion_table.num_angle_quantization = 2; double test_theta = 3.1415926; unsigned int expected_angular_bin = 1; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); @@ -320,17 +322,28 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = M_PI; + motion_table.num_angle_quantization = 2; double test_theta = M_PI; - unsigned int expected_angular_bin = 1; + unsigned int expected_angular_bin = 0; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } { motion_table.bin_size = M_PI; + motion_table.num_angle_quantization = 2; float test_theta = M_PI; unsigned int expected_angular_bin = 1; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } + + { + motion_table.bin_size = 0.0872664675; + motion_table.num_angle_quantization = 72; + double test_theta = 6.28318526567925; + unsigned int expected_angular_bin = 71; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } } diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index d55321b3f3..1fea5a11bc 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.2.7 + 1.2.8 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index bbdef57a8f..105446946a 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.2.7 + 1.2.8 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index b58cd83095..c945c59494 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.7 + 1.2.8 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 6da348c5f1..7f1ef07157 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -205,6 +205,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode // Connection to tell that server is still up std::unique_ptr bond_{nullptr}; + double bond_heartbeat_period; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/validate_messages.hpp b/nav2_util/include/nav2_util/validate_messages.hpp new file mode 100644 index 0000000000..58594f1ecd --- /dev/null +++ b/nav2_util/include/nav2_util/validate_messages.hpp @@ -0,0 +1,179 @@ +// Copyright (c) 2024 GoesM +// +// 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. + + +#ifndef NAV2_UTIL__VALIDATE_MESSAGES_HPP_ +#define NAV2_UTIL__VALIDATE_MESSAGES_HPP_ + +#include +#include + +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + + +// @brief Validation Check +// Check recieved message is safe or not for the nav2-system +// For each msg-type known in nav2, we could check it as following: +// if(!validateMsg()) RCLCPP_ERROR(,"malformed msg. Rejecting.") +// +// Workflow of validateMsg(): +// if here's a sub-msg-type in the recieved msg, +// the content of sub-msg would be checked as sub-msg-type +// then, check the whole recieved msg. +// +// Following conditions are involved in check: +// 1> Value Check: to avoid damaged value like like `nan`, `INF`, empty string and so on +// 2> Logic Check: to avoid value with bad logic, +// like the size of `map` should be equal to `height*width` +// 3> Any other needed condition could be joint here in future + +namespace nav2_util +{ + + +bool validateMsg(const double & num) +{ + /* @brief double/float value check + * if here'a need to check message validation + * it should be avoid to use double value like `nan`, `inf` + * otherwise, we regard it as an invalid message + */ + if (std::isinf(num)) {return false;} + if (std::isnan(num)) {return false;} + return true; +} + +template +bool validateMsg(const std::array & msg) +{ + /* @brief value check for double-array + * like the field `covariance` used in the msg-type: + * geometry_msgs::msg::PoseWithCovarianceStamped + */ + for (const auto & element : msg) { + if (!validateMsg(element)) {return false;} + } + + return true; +} + +const int NSEC_PER_SEC = 1e9; // 1 second = 1e9 nanosecond +bool validateMsg(const builtin_interfaces::msg::Time & msg) +{ + if (msg.nanosec >= NSEC_PER_SEC) { + return false; // invalid nanosec-stamp + } + return true; +} + +bool validateMsg(const std_msgs::msg::Header & msg) +{ + // check sub-type + if (!validateMsg(msg.stamp)) {return false;} + + /* @brief frame_id check + * if here'a need to check message validation + * it should at least have a non-empty frame_id + * otherwise, we regard it as an invalid message + */ + if (msg.frame_id.empty()) {return false;} + return true; +} + +bool validateMsg(const geometry_msgs::msg::Point & msg) +{ + // check sub-type + if (!validateMsg(msg.x)) {return false;} + if (!validateMsg(msg.y)) {return false;} + if (!validateMsg(msg.z)) {return false;} + return true; +} + +const double epsilon = 1e-4; +bool validateMsg(const geometry_msgs::msg::Quaternion & msg) +{ + // check sub-type + if (!validateMsg(msg.x)) {return false;} + if (!validateMsg(msg.y)) {return false;} + if (!validateMsg(msg.z)) {return false;} + if (!validateMsg(msg.w)) {return false;} + + if (abs(msg.x * msg.x + msg.y * msg.y + msg.z * msg.z + msg.w * msg.w - 1.0) >= epsilon) { + return false; + } + + return true; +} + +bool validateMsg(const geometry_msgs::msg::Pose & msg) +{ + // check sub-type + if (!validateMsg(msg.position)) {return false;} + if (!validateMsg(msg.orientation)) {return false;} + return true; +} + +bool validateMsg(const geometry_msgs::msg::PoseWithCovariance & msg) +{ + // check sub-type + if (!validateMsg(msg.pose)) {return false;} + if (!validateMsg(msg.covariance)) {return false;} + + return true; +} + +bool validateMsg(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + // check sub-type + if (!validateMsg(msg.header)) {return false;} + if (!validateMsg(msg.pose)) {return false;} + return true; +} + + +// Function to verify map meta information +bool validateMsg(const nav_msgs::msg::MapMetaData & msg) +{ + // check sub-type + if (!validateMsg(msg.origin)) {return false;} + if (!validateMsg(msg.resolution)) {return false;} + + // logic check + // 1> we don't need an empty map + if (msg.height == 0 || msg.width == 0) {return false;} + return true; +} + +// for msg-type like map, costmap and others as `OccupancyGrid` +bool validateMsg(const nav_msgs::msg::OccupancyGrid & msg) +{ + // check sub-type + if (!validateMsg(msg.header)) {return false;} + // msg.data : @todo any check for it ? + if (!validateMsg(msg.info)) {return false;} + + // check logic + if (msg.data.size() != msg.info.width * msg.info.height) { + return false; // check map-size + } + return true; +} + + +} // namespace nav2_util + + +#endif // NAV2_UTIL__VALIDATE_MESSAGES_HPP_ diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 298b94eedc..544360d5ad 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.2.7 + 1.2.8 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index e09ae54b5d..5976d098a8 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -19,6 +19,7 @@ #include #include "lifecycle_msgs/msg/state.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_util { @@ -35,6 +36,10 @@ LifecycleNode::LifecycleNode( rclcpp::Parameter( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); + nav2_util::declare_parameter_if_not_declared( + this, "bond_heartbeat_period", rclcpp::ParameterValue(0.1)); + this->get_parameter("bond_heartbeat_period", bond_heartbeat_period); + printLifecycleNodeNotification(); register_rcl_preshutdown_callback(); @@ -55,16 +60,18 @@ LifecycleNode::~LifecycleNode() void LifecycleNode::createBond() { - RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name()); + if (bond_heartbeat_period > 0.0) { + RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name()); - bond_ = std::make_unique( - std::string("bond"), - this->get_name(), - shared_from_this()); + bond_ = std::make_unique( + std::string("bond"), + this->get_name(), + shared_from_this()); - bond_->setHeartbeatPeriod(0.10); - bond_->setHeartbeatTimeout(4.0); - bond_->start(); + bond_->setHeartbeatPeriod(bond_heartbeat_period); + bond_->setHeartbeatTimeout(4.0); + bond_->start(); + } } void LifecycleNode::runCleanups() @@ -110,10 +117,12 @@ void LifecycleNode::register_rcl_preshutdown_callback() void LifecycleNode::destroyBond() { - RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); + if (bond_heartbeat_period > 0.0) { + RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); - if (bond_) { - bond_.reset(); + if (bond_) { + bond_.reset(); + } } } diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 9f1ae99bbc..ad0a5c0c41 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,3 +41,7 @@ target_link_libraries(test_odometry_utils ${library_name}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) + +ament_add_gtest(test_validation_messages test_validation_messages.cpp) +ament_target_dependencies(test_validation_messages rclcpp_lifecycle) +target_link_libraries(test_validation_messages ${library_name}) diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp new file mode 100644 index 0000000000..a7d8ed8904 --- /dev/null +++ b/nav2_util/test/test_validation_messages.cpp @@ -0,0 +1,368 @@ +// Copyright (c) 2024 GoesM +// +// 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 "nav2_util/validate_messages.hpp" + +TEST(ValidateMessagesTest, DoubleValueCheck) { + // Test valid double value + EXPECT_TRUE(nav2_util::validateMsg(3.14)); + // Test invalid double value (infinity) + EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits::infinity())); + // Test invalid double value (NaN) + EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits::quiet_NaN())); +} + +TEST(ValidateMessagesTest, TimeStampCheck) +{ + // Test valid time stamp + builtin_interfaces::msg::Time valid_time_stamp; + valid_time_stamp.sec = 123; + valid_time_stamp.nanosec = 456789; + EXPECT_TRUE(nav2_util::validateMsg(valid_time_stamp)); + // Test invalid time stamp (nanosec out of range) + builtin_interfaces::msg::Time invalid_time_stamp; + invalid_time_stamp.sec = 123; + invalid_time_stamp.nanosec = 1e9; // 1 second = 1e9 nanoseconds + EXPECT_FALSE(nav2_util::validateMsg(invalid_time_stamp)); +} + +TEST(ValidateMessagesTest, HeaderCheck) +{ + // Test valid header with non-empty frame_id + std_msgs::msg::Header valid_header; + valid_header.stamp.sec = 123; + valid_header.stamp.nanosec = 456789; + valid_header.frame_id = "map"; + EXPECT_TRUE(nav2_util::validateMsg(valid_header)); + // Test invalid header with empty frame_id + std_msgs::msg::Header invalid_header; + invalid_header.stamp.sec = 123; + invalid_header.stamp.nanosec = 456789; + invalid_header.frame_id = ""; + EXPECT_FALSE(nav2_util::validateMsg(invalid_header)); + invalid_header.stamp.sec = 123; + invalid_header.stamp.nanosec = 1e9; + invalid_header.frame_id = "map"; + EXPECT_FALSE(nav2_util::validateMsg(invalid_header)); +} + +TEST(ValidateMessagesTest, PointCheck) +{ + // Test valid Point message + geometry_msgs::msg::Point valid_point; + valid_point.x = 1.0; + valid_point.y = 2.0; + valid_point.z = 3.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_point)); + // Test invalid Point message with NaN value + geometry_msgs::msg::Point invalid_point; + invalid_point.x = 1.0; + invalid_point.y = std::numeric_limits::quiet_NaN(); + invalid_point.z = 3.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); + // Test invalid Point message with NaN value + invalid_point.x = std::numeric_limits::quiet_NaN(); + invalid_point.y = 2.0; + invalid_point.z = 3.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); + // Test invalid Point message with NaN value + invalid_point.x = 1.0; + invalid_point.y = 2.0; + invalid_point.z = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); +} + +TEST(ValidateMessagesTest, QuaternionCheck) +{ + // Test valid Quaternion message + geometry_msgs::msg::Quaternion valid_quaternion; + valid_quaternion.x = 0.0; + valid_quaternion.y = 0.0; + valid_quaternion.z = 0.0; + valid_quaternion.w = 1.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_quaternion)); + // Test invalid Quaternion message with invalid magnitude + geometry_msgs::msg::Quaternion invalid_quaternion; + invalid_quaternion.x = 0.1; + invalid_quaternion.y = 0.2; + invalid_quaternion.z = 0.3; + invalid_quaternion.w = 0.5; // Invalid magnitude (should be 1.0) + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + + // One NaN value + invalid_quaternion.x = 0.0; + invalid_quaternion.y = std::numeric_limits::quiet_NaN(); + invalid_quaternion.z = 0.0; + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = std::numeric_limits::quiet_NaN(); + invalid_quaternion.y = 0.0; + invalid_quaternion.z = 0.0; + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = 0.0; + invalid_quaternion.y = 0.0; + invalid_quaternion.z = std::numeric_limits::quiet_NaN(); + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = 0.0; + invalid_quaternion.y = 0.0; + invalid_quaternion.z = 1.0; + invalid_quaternion.w = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); +} + +TEST(ValidateMessagesTest, PoseCheck) +{ + // Test valid Pose message + geometry_msgs::msg::Pose valid_pose; + valid_pose.position.x = 1.0; + valid_pose.position.y = 2.0; + valid_pose.position.z = 3.0; + valid_pose.orientation.x = 1.0; + valid_pose.orientation.y = 0.0; + valid_pose.orientation.z = 0.0; + valid_pose.orientation.w = 0.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_pose)); + // Test invalid Pose message with invalid position + geometry_msgs::msg::Pose invalid_pose; + invalid_pose.position.x = 1.0; + invalid_pose.position.y = std::numeric_limits::quiet_NaN(); + invalid_pose.position.z = 3.0; + invalid_pose.orientation.x = 1.0; + invalid_pose.orientation.y = 0.0; + invalid_pose.orientation.z = 0.0; + invalid_pose.orientation.w = 0.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); + // Test invalid Pose message with invalid orientation + invalid_pose.position.x = 1.0; + invalid_pose.position.y = 2.0; + invalid_pose.position.z = 3.0; + invalid_pose.orientation.x = 0.1; + invalid_pose.orientation.y = 0.2; + invalid_pose.orientation.z = 0.3; + invalid_pose.orientation.w = 0.4; + EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); +} + + +TEST(ValidateMessagesTest, MapMetaDataCheck) { + // Test valid MapMetaData message + nav_msgs::msg::MapMetaData valid_map_meta_data; + valid_map_meta_data.resolution = 0.05; + valid_map_meta_data.width = 100; + valid_map_meta_data.height = 100; + geometry_msgs::msg::Pose valid_origin; + valid_origin.position.x = 0.0; + valid_origin.position.y = 0.0; + valid_origin.position.z = 0.0; + valid_origin.orientation.x = 0.0; + valid_origin.orientation.y = 0.0; + valid_origin.orientation.z = 0.0; + valid_origin.orientation.w = 1.0; + valid_map_meta_data.origin = valid_origin; + EXPECT_TRUE(nav2_util::validateMsg(valid_map_meta_data)); + + // Test invalid origin message + nav_msgs::msg::MapMetaData invalid_map_meta_data; + invalid_map_meta_data.resolution = 100.0; + invalid_map_meta_data.width = 100; + invalid_map_meta_data.height = 100; + geometry_msgs::msg::Pose invalid_origin; + invalid_origin.position.x = 0.0; + invalid_origin.position.y = 0.0; + invalid_origin.position.z = 0.0; + invalid_origin.orientation.x = 0.0; + invalid_origin.orientation.y = 0.0; + invalid_origin.orientation.z = 1.0; + invalid_origin.orientation.w = 1.0; + invalid_map_meta_data.origin = invalid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); + + // Test invalid resolution message + invalid_map_meta_data.resolution = std::numeric_limits::quiet_NaN(); + invalid_map_meta_data.width = 100; + invalid_map_meta_data.height = 100; + invalid_map_meta_data.origin = valid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); + + // Test invalid MapMetaData message with zero width + invalid_map_meta_data.resolution = 0.05; + invalid_map_meta_data.width = 0; + invalid_map_meta_data.height = 100; + invalid_map_meta_data.origin = valid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); +} + +TEST(ValidateMessagesTest, OccupancyGridCheck) { + // Test valid OccupancyGrid message + nav_msgs::msg::OccupancyGrid valid_occupancy_grid; + valid_occupancy_grid.header.frame_id = "map"; + valid_occupancy_grid.info.resolution = 0.05; + valid_occupancy_grid.info.width = 100; + valid_occupancy_grid.info.height = 100; + std::vector data(100 * 100, 0); // Initialize with zeros + valid_occupancy_grid.data = data; + EXPECT_TRUE(nav2_util::validateMsg(valid_occupancy_grid)); + + // Test invalid header message with wrong data size + nav_msgs::msg::OccupancyGrid invalid_occupancy_grid; + invalid_occupancy_grid.header.frame_id = ""; // Incorrect id + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 100; + invalid_occupancy_grid.info.height = 100; + invalid_occupancy_grid.data = data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid info message with wrong data size + invalid_occupancy_grid.header.frame_id = "map"; + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 0; // Incorrect width + invalid_occupancy_grid.info.height = 100; + invalid_occupancy_grid.data = data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid OccupancyGrid message with wrong data size + invalid_occupancy_grid.header.frame_id = "map"; + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 100; + invalid_occupancy_grid.info.height = 100; + std::vector invalid_data(100 * 99, 0); // Incorrect data size + invalid_occupancy_grid.data = invalid_data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); +} + +TEST(ValidateMessagesTest, PoseWithCovarianceCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovariance validate_msg; + validate_msg.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.covariance[35] = 0.06853891909122467; + + validate_msg.pose.position.x = 0.50010401010515571; + validate_msg.pose.position.y = 1.7468730211257935; + validate_msg.pose.position.z = 0.0; + + validate_msg.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.orientation.y = 0.0; + validate_msg.pose.orientation.z = 0.0; + validate_msg.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovariance invalidate_msg1; + invalidate_msg1.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.covariance[7] = NAN; + invalidate_msg1.covariance[9] = NAN; + invalidate_msg1.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.position.z = 0.0; + + invalidate_msg1.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.orientation.y = 0.0; + invalidate_msg1.pose.orientation.z = 0.0; + invalidate_msg1.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovariance invalidate_msg2; + invalidate_msg2.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.position.x = NAN; + invalidate_msg2.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.position.z = 0.0; + + invalidate_msg2.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.orientation.y = 0.0; + invalidate_msg2.pose.orientation.z = 0.0; + invalidate_msg2.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + +TEST(ValidateMessagesTest, PoseWithCovarianceStampedCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovarianceStamped validate_msg; + validate_msg.header.frame_id = "map"; + validate_msg.header.stamp.sec = 1711029956; + validate_msg.header.stamp.nanosec = 146734875; + + validate_msg.pose.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.pose.covariance[35] = 0.06853891909122467; + + validate_msg.pose.pose.position.x = 0.50010401010515571; + validate_msg.pose.pose.position.y = 1.7468730211257935; + validate_msg.pose.pose.position.z = 0.0; + + validate_msg.pose.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.pose.orientation.y = 0.0; + validate_msg.pose.pose.orientation.z = 0.0; + validate_msg.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg1; + invalidate_msg1.header.frame_id = "map"; + invalidate_msg1.header.stamp.sec = 1711029956; + invalidate_msg1.header.stamp.nanosec = 146734875; + + invalidate_msg1.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.pose.covariance[7] = NAN; + invalidate_msg1.pose.covariance[9] = NAN; + invalidate_msg1.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.pose.position.z = 0.0; + + invalidate_msg1.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.pose.orientation.y = 0.0; + invalidate_msg1.pose.pose.orientation.z = 0.0; + invalidate_msg1.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg2; + invalidate_msg2.header.frame_id = ""; + invalidate_msg2.header.stamp.sec = 1711029956; + invalidate_msg2.header.stamp.nanosec = 146734875; + + invalidate_msg2.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.pose.position.x = 0.50010401010515571; + invalidate_msg2.pose.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.pose.position.z = 0.0; + + invalidate_msg2.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.pose.orientation.y = 0.0; + invalidate_msg2.pose.pose.orientation.z = 0.0; + invalidate_msg2.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + + +// Add more test cases for other validateMsg functions if needed diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index c9a9775135..9928a77022 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.2.7 + 1.2.8 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index d981fac4e7..1f602689c9 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.2.7 + 1.2.8 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 c489b9f2a6..d53e79afed 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.2.7 + 1.2.8 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index b726fb2c27..9a0d7012b3 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.7 + 1.2.8 ROS2 Navigation Stack