From ddd1b4d49209722cbd4400ea3eadacae0a17e024 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 23 Aug 2024 17:16:48 -0700 Subject: [PATCH] Iron Sync 8: August 23, 2024 (#4645) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * earlier `executor_thread_.reset()` in nav2_costmap_ros (#4385) * reorder cleanup_queue Signed-off-by: goes * codestyle Signed-off-by: goes * codestyle Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes * [RotationShimController] Fix test for rotate to goal heading (#4289) (#4391) * Fix rotate to goal heading tests Signed-off-by: Antoine Gennart * Update bt_service_node.hpp debug msg (#4398) Provide service_name instead of service_node_name for debugging bt_service_node Signed-off-by: João Britto * reset laser_scan_filter before reinit (#4397) Signed-off-by: goes Co-authored-by: goes * Fix error messages (#4411) Signed-off-by: Christoph Froehlich * Warn if inflation_radius_ < inscribed_radius_ (#4423) * Warn if inflation_radius_ < inscribed_radius_ Signed-off-by: Tony Najjar * convert to error Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar * chore: cleanup ros1 leftovers (#4446) Signed-off-by: Rein Appeldoorn * precomputeDistanceHeuristic is now computed once (#4451) Signed-off-by: Vincent Belpois Co-authored-by: SiddharthaUpase * shutdown services in destructor of `ClearCostmapService` (#4495) Signed-off-by: GoesM_server Co-authored-by: GoesM_server * Fix world to map coordinate conversion (#4506) Signed-off-by: HovorunBh * fix(nav2_costmap_2d): make obstacle layer not current on enabled toggle (#4507) Signed-off-by: Kemal Bektas Co-authored-by: Kemal Bektas * min_turning_r_ getting param fix (#4510) * min_turning_r_ getting param fix Signed-off-by: Ivan Radionov * Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp Signed-off-by: Steve Macenski Signed-off-by: Ivan Radionov --------- Signed-off-by: Ivan Radionov Signed-off-by: Steve Macenski Co-authored-by: Ivan Radionov Co-authored-by: Steve Macenski * Return out of map update if frames mismatch. Signed-off-by Joey Yang (#4517) Signed-off-by: Joey Yang * check nullptr in smoothPlan() (#4544) * check nullptr in smoothPlan() Signed-off-by: GoesM * code-style Signed-off-by: GoesM * code-style Signed-off-by: GoesM * simple change Signed-off-by: GoesM --------- Signed-off-by: GoesM Co-authored-by: GoesM * check nullPtr in `computeControl()` (#4548) Signed-off-by: goes Co-authored-by: goes * bumping to 1.2.10 for release * fixing merge conflict iron --------- Signed-off-by: goes Signed-off-by: Antoine Gennart Signed-off-by: João Britto Signed-off-by: Christoph Froehlich Signed-off-by: Tony Najjar Signed-off-by: Rein Appeldoorn Signed-off-by: Vincent Belpois Signed-off-by: GoesM_server Signed-off-by: HovorunBh Signed-off-by: Kemal Bektas Signed-off-by: Ivan Radionov Signed-off-by: Steve Macenski Signed-off-by: Joey Yang Signed-off-by: GoesM Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: goes Co-authored-by: Saitama Co-authored-by: João Britto Co-authored-by: Christoph Fröhlich Co-authored-by: Tony Najjar Co-authored-by: Rein Appeldoorn Co-authored-by: Vincent <46542431+VincidaB@users.noreply.github.com> Co-authored-by: SiddharthaUpase Co-authored-by: Bohdan <72872431+HovorunBh@users.noreply.github.com> Co-authored-by: Kemal Bektas <34746077+bektaskemal@users.noreply.github.com> Co-authored-by: Kemal Bektas Co-authored-by: Ivan Radionov <45877502+JJRedmond@users.noreply.github.com> Co-authored-by: Ivan Radionov Co-authored-by: Joey Yang --- nav2_amcl/package.xml | 2 +- nav2_amcl/src/amcl_node.cpp | 1 + .../nav2_behavior_tree/bt_action_node.hpp | 2 +- .../bt_cancel_action_node.hpp | 4 +- .../nav2_behavior_tree/bt_service_node.hpp | 6 +-- nav2_behavior_tree/package.xml | 2 +- nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_controller/src/controller_server.cpp | 13 ++++-- nav2_core/package.xml | 2 +- nav2_costmap_2d/cfg/Costmap2D.cfg | 23 ---------- nav2_costmap_2d/cfg/GenericPlugin.cfg | 7 --- nav2_costmap_2d/cfg/InflationPlugin.cfg | 12 ------ nav2_costmap_2d/cfg/ObstaclePlugin.cfg | 22 ---------- nav2_costmap_2d/cfg/VoxelPlugin.cfg | 22 ---------- .../nav2_costmap_2d/clear_costmap_service.hpp | 5 +++ nav2_costmap_2d/launch/example.launch | 21 --------- nav2_costmap_2d/launch/example_params.yaml | 43 ------------------- nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/plugins/inflation_layer.cpp | 10 +++++ nav2_costmap_2d/plugins/obstacle_layer.cpp | 5 ++- nav2_costmap_2d/plugins/static_layer.cpp | 1 + nav2_costmap_2d/src/clear_costmap_service.cpp | 8 ++++ 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 +- .../dwb_plugins/cfg/KinematicParams.cfg | 33 -------------- 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/motion_models.hpp | 4 +- nav2_mppi_controller/package.xml | 2 +- nav2_mppi_controller/src/optimizer.cpp | 2 +- nav2_mppi_controller/test/critics_tests.cpp | 2 +- .../test/motion_model_tests.cpp | 2 +- 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 +- .../test/test_shim_controller.cpp | 22 +++++++--- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- .../include/nav2_smac_planner/a_star.hpp | 1 + .../include/nav2_smac_planner/utils.hpp | 4 +- nav2_smac_planner/package.xml | 2 +- nav2_smac_planner/src/a_star.cpp | 6 ++- nav2_smac_planner/test/test_utils.cpp | 9 ++++ nav2_smoother/package.xml | 2 +- nav2_smoother/src/nav2_smoother.cpp | 8 +++- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 67 files changed, 128 insertions(+), 249 deletions(-) delete mode 100755 nav2_costmap_2d/cfg/Costmap2D.cfg delete mode 100755 nav2_costmap_2d/cfg/GenericPlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/InflationPlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/ObstaclePlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/VoxelPlugin.cfg delete mode 100644 nav2_costmap_2d/launch/example.launch delete mode 100644 nav2_costmap_2d/launch/example_params.yaml delete mode 100644 nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 3effde7d18..0be0ee66ec 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.2.9 + 1.2.10

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 6e92626fc9..11870318ce 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1367,6 +1367,7 @@ AmclNode::dynamicParametersCallback( lasers_update_.clear(); frame_to_laser_.clear(); laser_scan_connection_.disconnect(); + laser_scan_filter_.reset(); laser_scan_sub_.reset(); initMessageFilters(); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index b9246f16c0..dbd6090e49 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -100,7 +100,7 @@ class BtActionNode : public BT::ActionNodeBase RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for %f s", + node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs", action_name.c_str(), wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index 7831f533bc..9b9ff390ec 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -93,8 +93,8 @@ class BtCancelActionNode : public BT::ActionNodeBase RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", - action_name.c_str()); + node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs", + action_name.c_str(), wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string("Action server ") + action_name + std::string(" not available")); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 0f7c52bd3b..0fd2ac0d23 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -84,12 +84,12 @@ class BtServiceNode : public BT::ActionNodeBase service_name_.c_str()); if (!service_client_->wait_for_service(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" service server not available after waiting for 1 s", - service_node_name.c_str()); + node_->get_logger(), "\"%s\" service server not available after waiting for %.2fs", + service_name_.c_str(), wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string( "Service server %s not available", - service_node_name.c_str())); + service_name_.c_str())); } RCLCPP_DEBUG( diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 48c8bd2417..63dbdd4839 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.2.9 + 1.2.10 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 9f2b9ad3ff..ba0725850f 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.2.9 + 1.2.10 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 5bf57af203..cba47a2f88 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.9 + 1.2.10 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 4e708a9e39..38ac70767b 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.2.9 + 1.2.10 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index f59f2f7738..2ed96f28e6 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.2.9 + 1.2.10 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 615d8ce78a..f718c363cd 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.2.9 + 1.2.10 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 172572b73a..a8f210f94e 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.2.9 + 1.2.10 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 4ebb396c7d..677e56c890 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.2.9 + 1.2.10 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 01c529b923..9dd6c118ea 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -436,7 +436,12 @@ void ControllerServer::computeControl() RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { - std::string c_name = action_server_->get_current_goal()->controller_id; + auto goal = action_server_->get_current_goal(); + if (!goal) { + return; // goal would be nullptr if action_server_ is inactivate. + } + + std::string c_name = goal->controller_id; std::string current_controller; if (findControllerId(c_name, current_controller)) { current_controller_ = current_controller; @@ -444,7 +449,7 @@ void ControllerServer::computeControl() throw nav2_core::InvalidController("Failed to find controller name: " + c_name); } - std::string gc_name = action_server_->get_current_goal()->goal_checker_id; + std::string gc_name = goal->goal_checker_id; std::string current_goal_checker; if (findGoalCheckerId(gc_name, current_goal_checker)) { current_goal_checker_ = current_goal_checker; @@ -452,7 +457,7 @@ void ControllerServer::computeControl() throw nav2_core::ControllerException("Failed to find goal checker name: " + gc_name); } - std::string pc_name = action_server_->get_current_goal()->progress_checker_id; + std::string pc_name = goal->progress_checker_id; std::string current_progress_checker; if (findProgressCheckerId(pc_name, current_progress_checker)) { current_progress_checker_ = current_progress_checker; @@ -460,7 +465,7 @@ void ControllerServer::computeControl() throw nav2_core::ControllerException("Failed to find progress checker name: " + pc_name); } - setPlannerPath(action_server_->get_current_goal()->path); + setPlannerPath(goal->path); progress_checkers_[current_progress_checker_]->reset(); last_valid_cmd_time_ = now(); diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 2e6a02f648..8ae83228a0 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.2.9 + 1.2.10 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/cfg/Costmap2D.cfg b/nav2_costmap_2d/cfg/Costmap2D.cfg deleted file mode 100755 index 07c4a1628b..0000000000 --- a/nav2_costmap_2d/cfg/Costmap2D.cfg +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, str_t - -gen = ParameterGenerator() - -gen.add("transform_tolerance", double_t, 0, "Specifies the delay in transform (tf) data that is tolerable in seconds.", 0.3, 0, 10) -gen.add("update_frequency", double_t, 0, "The frequency in Hz for the map to be updated.", 5, 0, 100) -gen.add("publish_frequency", double_t, 0, "The frequency in Hz for the map to be publish display information.", 0, 0, 100) - -#map params -gen.add("width", int_t, 0, "The width of the map in meters.", 10, 0) -gen.add("height", int_t, 0, "The height of the map in meters.", 10, 0) -gen.add("resolution", double_t, 0, "The resolution of the map in meters/cell.", 0.05, 0, 50) -gen.add("origin_x", double_t, 0, "The x origin of the map in the global frame in meters.", 0) -gen.add("origin_y", double_t, 0, "The y origin of the map in the global frame in meters.", 0) - -# robot footprint shape -gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robot_base_frame coordinate frame as a list in the format: [ [x1, y1], [x2, y2], ...., [xn, yn] ].", "[]") -gen.add("robot_radius", double_t, 0, 'The radius of the robot in meters, this parameter should only be set for circular robots, all others should use the footprint parameter described above.', 0.46, 0, 10) -gen.add("footprint_padding", double_t, 0, "How much to pad (increase the size of) the footprint, in meters.", 0.01) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "Costmap2D")) diff --git a/nav2_costmap_2d/cfg/GenericPlugin.cfg b/nav2_costmap_2d/cfg/GenericPlugin.cfg deleted file mode 100755 index 555e2b5415..0000000000 --- a/nav2_costmap_2d/cfg/GenericPlugin.cfg +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t - -gen = ParameterGenerator() -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "GenericPlugin")) diff --git a/nav2_costmap_2d/cfg/InflationPlugin.cfg b/nav2_costmap_2d/cfg/InflationPlugin.cfg deleted file mode 100755 index 5c11eaf791..0000000000 --- a/nav2_costmap_2d/cfg/InflationPlugin.cfg +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost values during inflation.", 10, 0, 100) -gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50) -gen.add("inflate_unknown", bool_t, 0, "Whether to inflate unknown cells.", False) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "InflationPlugin")) diff --git a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg deleted file mode 100755 index a94e98fbbe..0000000000 --- a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) -gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50) - -combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing")], - "Method for combining layers enum") -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum) - - -# gen.add("obstacle_max_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50) -# gen.add("obstacle_min_range", double_t, 0, "The default minimum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 0.0, 0, 50) -# gen.add("raytrace_max_range", double_t, 0, "The default maximum range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) -# gen.add("raytrace_min_range", double_t, 0, "The default minimum range in meters from which to raytrace out obstacles from the map using sensor data.", 0.0, 0, 50) -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "ObstaclePlugin")) diff --git a/nav2_costmap_2d/cfg/VoxelPlugin.cfg b/nav2_costmap_2d/cfg/VoxelPlugin.cfg deleted file mode 100755 index 977fcb99d4..0000000000 --- a/nav2_costmap_2d/cfg/VoxelPlugin.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to use this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) -gen.add("max_obstacle_height", double_t, 0, "Max Obstacle Height", 2.0, 0, 50) -gen.add("origin_z", double_t, 0, "The z origin of the map in meters.", 0, 0) -gen.add("z_resolution", double_t, 0, "The z resolution of the map in meters/cell.", 0.2, 0, 50) -gen.add("z_voxels", int_t, 0, "The number of voxels to in each vertical column.", 10, 0, 16) -gen.add("unknown_threshold", int_t, 0, 'The number of unknown cells allowed in a column considered to be known', 15, 0, 16) -gen.add("mark_threshold", int_t, 0, 'The maximum number of marked cells allowed in a column considered to be free', 0, 0, 16) - -combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing")], - "Method for combining layers enum") -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, 0, 2, edit_method=combo_enum) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "VoxelPlugin")) 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 2d74530b43..844902dd10 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 @@ -48,6 +48,11 @@ class ClearCostmapService */ ClearCostmapService() = delete; + /** + * @brief A destructor + */ + ~ClearCostmapService(); + /** * @brief Clears the region outside of a user-specified area reverting to the static map */ diff --git a/nav2_costmap_2d/launch/example.launch b/nav2_costmap_2d/launch/example.launch deleted file mode 100644 index ac089abfba..0000000000 --- a/nav2_costmap_2d/launch/example.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/nav2_costmap_2d/launch/example_params.yaml b/nav2_costmap_2d/launch/example_params.yaml deleted file mode 100644 index 72bf695fad..0000000000 --- a/nav2_costmap_2d/launch/example_params.yaml +++ /dev/null @@ -1,43 +0,0 @@ -global_frame: map -robot_base_frame: base_link -update_frequency: 5.0 -publish_frequency: 1.0 - -#set if you want the voxel map published -publish_voxel_map: true - -#set to true if you want to initialize the costmap from a static map -static_map: false - -#begin - COMMENT these lines if you set static_map to true -rolling_window: true -width: 6.0 -height: 6.0 -resolution: 0.025 -#end - COMMENT these lines if you set static_map to true - -#START VOXEL STUFF -map_type: voxel -origin_z: 0.0 -z_resolution: 0.2 -z_voxels: 10 -unknown_threshold: 10 -mark_threshold: 0 -#END VOXEL STUFF - -transform_tolerance: 0.3 -obstacle_max_range: 2.5 -obstacle_min_range: 0.0 -max_obstacle_height: 2.0 -raytrace_max_range: 3.0 -raytrace_min_range: 0.0 - -footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] -#robot_radius: 0.46 -footprint_padding: 0.01 -inflation_radius: 0.55 -cost_scaling_factor: 10.0 -lethal_cost_threshold: 100 -observation_sources: base_scan -base_scan: {data_type: LaserScan, expected_update_rate: 0.4, - observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index fd720fd24f..3932748e7e 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.2.9 + 1.2.10 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/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 067877e548..90068f463f 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -170,6 +170,16 @@ InflationLayer::onFootprintChanged() computeCaches(); need_reinflation_ = true; + if (inflation_radius_ < inscribed_radius_) { + RCLCPP_ERROR( + logger_, + "The configured inflation radius (%.3f) is smaller than " + "the computed inscribed radius (%.3f) of your footprint, " + "it is highly recommended to set inflation radius to be at " + "least as big as the inscribed radius to avoid collisions", + inflation_radius_, inscribed_radius_); + } + RCLCPP_DEBUG( logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu," " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 653c0885ca..4d5f3c717b 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -304,8 +304,11 @@ ObstacleLayer::dynamicParametersCallback( max_obstacle_height_ = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { - if (param_name == name_ + "." + "enabled") { + if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { enabled_ = parameter.as_bool(); + if (enabled_) { + current_ = false; + } } else if (param_name == name_ + "." + "footprint_clearing_enabled") { footprint_clearing_enabled_ = parameter.as_bool(); } diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 729530d82b..25a10e3bd8 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -318,6 +318,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u "StaticLayer: Map update ignored. Current map is in frame %s " "but update was in frame %s", map_frame_.c_str(), update->header.frame_id.c_str()); + return; } unsigned int di = 0; diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 857278b841..96b3f1bc5f 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -59,6 +59,14 @@ ClearCostmapService::ClearCostmapService( std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } +ClearCostmapService::~ClearCostmapService() +{ + // make sure services shutdown. + clear_except_service_.reset(); + clear_around_service_.reset(); + clear_entire_service_.reset(); +} + void ClearCostmapService::clearExceptRegionCallback( const shared_ptr/*request_header*/, const shared_ptr request, diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 1294ee0a14..8d882d6ea7 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -352,6 +352,7 @@ nav2_util::CallbackReturn Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); + executor_thread_.reset(); costmap_publisher_.reset(); clear_costmap_service_.reset(); @@ -366,8 +367,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); - - executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 83c0619c52..14b310d0db 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.9 + 1.2.10 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 61e4679cdf..6fdc74af28 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.9 + 1.2.10 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 491ee4ecb7..5cdae049f1 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.9 + 1.2.10 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 a987b73763..e5c32f78da 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.9 + 1.2.10 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg b/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg deleted file mode 100644 index 51dab28aa4..0000000000 --- a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t - -gen = ParameterGenerator() - -# velocities -gen.add('min_vel_x', double_t, 0, 'The minimum x velocity for the robot in m/s', 0.0) -gen.add('max_vel_x', double_t, 0, 'The maximum x velocity for the robot in m/s', 0.55) -gen.add('min_vel_y', double_t, 0, 'The minimum y velocity for the robot in m/s', -0.1) -gen.add('max_vel_y', double_t, 0, 'The maximum y velocity for the robot in m/s', 0.1) -gen.add('max_vel_theta', double_t, 0, 'The absolute value of the maximum rotational velocity for the robot in rad/s. ' - 'The minimum rotational velocity is assumed to be -max_vel_theta', 1.0) - -# acceleration -gen.add('acc_lim_x', double_t, 0, 'The acceleration limit of the robot in the x direction in m/s^2', 2.5) -gen.add('acc_lim_y', double_t, 0, 'The acceleration limit of the robot in the y direction in m/s^2', 2.5) -gen.add('acc_lim_theta', double_t, 0, 'The acceleration limit of the robot in the theta direction in rad/s^2', 3.2) - -gen.add('decel_lim_x', double_t, 0, 'The deceleration limit of the robot in the x direction in m/s^2', -2.5) -gen.add('decel_lim_y', double_t, 0, 'The deceleration limit of the robot in the y direction in m/s^2', -2.5) -gen.add('decel_lim_theta', double_t, 0, 'The deceleration limit of the robot in the theta direction in rad/s^2', -3.2) - -gen.add('min_speed_xy', double_t, 0, 'The absolute value of the minimum translational/xy velocity in m/s. ' - 'If the value is negative, then the min speed will be arbitrarily close to 0.0. ' - 'Previously called min_trans_vel', 0.1) -gen.add('max_speed_xy', double_t, 0, 'The absolute value of the maximum translational/xy velocity in m/s. ' - 'If the value is negative, then the max speed is hypot(max_vel_x, max_vel_y). ' - 'Previously called max_trans_vel', 0.55) -gen.add('min_speed_theta', double_t, 0, 'The absolute value of the minimum rotational velocity in rad/s. ' - 'If the value is negative, then the min speed will be arbitrarily close to 0.0.' - ' Previously called min_rot_vel', 0.4) - -exit(gen.generate('dwb_plugins', 'dwb_plugins', 'KinematicParams')) diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 4449747736..7e4e473cf2 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.9 + 1.2.10 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 f0e5f2832b..381df229bb 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.9 + 1.2.10 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 5e0a9e990c..824c9ce4e7 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.9 + 1.2.10 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 305ff7b75e..db977f78a9 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.9 + 1.2.10 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 85258fe230..b41d116ef0 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.2.9 + 1.2.10 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 a7ba7c6281..8700c1d65f 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.2.9 + 1.2.10 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 4fe85bfb05..0c5671ea3e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -88,9 +88,9 @@ class AckermannMotionModel : public MotionModel /** * @brief Constructor for mppi::AckermannMotionModel */ - explicit AckermannMotionModel(ParametersHandler * param_handler) + explicit AckermannMotionModel(ParametersHandler * param_handler, const std::string & name) { - auto getParam = param_handler->getParamGetter("AckermannConstraints"); + auto getParam = param_handler->getParamGetter(name + ".AckermannConstraints"); getParam(min_turning_r_, "min_turning_r", 0.2); } diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 33c9af4085..dd2be79bd2 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.2.9 + 1.2.10 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3c5d203289..86359395ef 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -411,7 +411,7 @@ void Optimizer::setMotionModel(const std::string & model) } else if (model == "Omni") { motion_model_ = std::make_shared(); } else if (model == "Ackermann") { - motion_model_ = std::make_shared(parameters_handler_); + motion_model_ = std::make_shared(parameters_handler_, name_); } else { throw nav2_core::ControllerException( std::string( diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index eb2a79fab7..bfa912dbd7 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -117,7 +117,7 @@ TEST(CriticTests, ConstraintsCritic) // Now with ackermann, all in constraint so no costs to score state.vx = 0.40 * xt::ones({1000, 30}); state.wz = 1.5 * xt::ones({1000, 30}); - data.motion_model = std::make_shared(¶m_handler); + data.motion_model = std::make_shared(¶m_handler, node->get_name()); critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 6085896cfe..4238fa03fb 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -136,7 +136,7 @@ TEST(MotionModelTests, AckermannTest) auto node = std::make_shared("my_node"); ParametersHandler param_handler(node); std::unique_ptr model = - std::make_unique(¶m_handler); + std::make_unique(¶m_handler, node->get_name()); // Check that predict properly populates the trajectory velocities with the control velocities state.cvx = 10 * xt::ones({batches, timesteps}); diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 120a592e71..9ec0c988fc 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.2.9 + 1.2.10 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 6fcdf170db..f1f5085628 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.2.9 + 1.2.10 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index ed091d614b..92d596b253 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.2.9 + 1.2.10 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 2b2a6ff96b..36644e4da2 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.9 + 1.2.10 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 59bc9b0fda..47d4b0577a 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.9 + 1.2.10 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 307f4385d9..ce17630e75 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -343,13 +343,16 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { // Test state update and path setting nav_msgs::msg::Path path; - path.header.frame_id = "fake_frame"; + path.header.frame_id = "base_link"; path.poses.resize(4); geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = "base_link"; geometry_msgs::msg::Twist velocity; nav2_controller::SimpleGoalChecker checker; + node->declare_parameter( + "checker.xy_goal_tolerance", + 1.0); checker.initialize(node, "checker", costmap); path.header.frame_id = "base_link"; @@ -359,15 +362,24 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { path.poses[1].pose.position.y = 0.05; path.poses[2].pose.position.x = 0.10; path.poses[2].pose.position.y = 0.10; + // goal position within checker xy_goal_tolerance path.poses[3].pose.position.x = 0.20; path.poses[3].pose.position.y = 0.20; + // goal heading 45 degrees to the left + path.poses[3].pose.orientation.z = -0.3826834; + path.poses[3].pose.orientation.w = 0.9238795; path.poses[3].header.frame_id = "base_link"; - // this should make the goal checker to validated the fact that the robot is in range - // of the goal. The rotation shim controller should rotate toward the goal heading - // then it will throw an exception because the costmap is bogus controller->setPlan(path); - EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); + + // goal heading 45 degrees to the right + path.poses[3].pose.orientation.z = 0.3826834; + path.poses[3].pose.orientation.w = 0.9238795; + controller->setPlan(path); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } TEST(RotationShimControllerTest, testDynamicParameter) diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 7fb66af57a..0300bad3a8 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.2.9 + 1.2.10 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 f6c056fdd4..685b16a672 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.2.9 + 1.2.10 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index eca1a65d81..ad39379d29 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -240,6 +240,7 @@ class AStarAlgorithm int _timing_interval = 5000; bool _traverse_unknown; + bool _is_initialized; int _max_iterations; int _max_on_approach_iterations; double _max_planning_time; diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp index 277edb6083..9d715aeaa6 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -46,9 +46,9 @@ inline geometry_msgs::msg::Pose getWorldCoords( { geometry_msgs::msg::Pose msg; msg.position.x = - static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); + static_cast(costmap->getOriginX()) + (mx - 0.5) * costmap->getResolution(); msg.position.y = - static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); + static_cast(costmap->getOriginY()) + (my - 0.5) * costmap->getResolution(); return msg; } diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index c0b088c03c..bb9473ad0c 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.2.9 + 1.2.10 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 00517be873..aa22c4c2c9 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -36,6 +36,7 @@ AStarAlgorithm::AStarAlgorithm( const MotionModel & motion_model, const SearchInfo & search_info) : _traverse_unknown(true), + _is_initialized(false), _max_iterations(0), _max_planning_time(0), _x_size(0), @@ -67,7 +68,10 @@ void AStarAlgorithm::initialize( _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; _max_planning_time = max_planning_time; - NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + if(!_is_initialized) { + NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + } + _is_initialized = true; _dim3_size = dim_3_size; _expander = std::make_unique>( _motion_model, _search_info, _traverse_unknown, _dim3_size); diff --git a/nav2_smac_planner/test/test_utils.cpp b/nav2_smac_planner/test/test_utils.cpp index 8a6b2440ef..7dd105d0b3 100644 --- a/nav2_smac_planner/test/test_utils.cpp +++ b/nav2_smac_planner/test/test_utils.cpp @@ -147,3 +147,12 @@ TEST(create_marker, test_createMarker) EXPECT_EQ(marker2.id, 8u); EXPECT_EQ(marker2.points.size(), 0u); } + +TEST(convert_map_to_world_to_map, test_convert_map_to_world_to_map) +{ + auto costmap = nav2_costmap_2d::Costmap2D(10.0, 10.0, 0.05, 0.0, 0.0, 0); + + float mx = 200.0; + float my = 100.0; + geometry_msgs::msg::Pose pose = getWorldCoords(mx, my, &costmap); +} diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 93842a507e..057cbb22ed 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.2.9 + 1.2.10 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 8119c614f9..8522266250 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -265,7 +265,12 @@ void SmootherServer::smoothPlan() auto result = std::make_shared(); try { - std::string c_name = action_server_->get_current_goal()->smoother_id; + auto goal = action_server_->get_current_goal(); + if (!goal) { + return; // if action_server_ is inactivate, goal would be a nullptr + } + + std::string c_name = goal->smoother_id; std::string current_smoother; if (findSmootherId(c_name, current_smoother)) { current_smoother_ = current_smoother; @@ -274,7 +279,6 @@ void SmootherServer::smoothPlan() } // Perform smoothing - auto goal = action_server_->get_current_goal(); result->path = goal->path; if (!validate(result->path)) { diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 87d6024c08..c14f0dae28 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.2.9 + 1.2.10 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 167441d394..bf26f43f50 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.9 + 1.2.10 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 3e922c515e..45be0f96b4 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.2.9 + 1.2.10 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 17def7f4b6..74b70ffe97 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.2.9 + 1.2.10 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 0b67699b73..2969ad347c 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.2.9 + 1.2.10 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 e96d215fcf..858c5915c9 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.2.9 + 1.2.10 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index a4b681828f..98845fcfb5 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.9 + 1.2.10 ROS2 Navigation Stack