diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 3effde7d189..0be0ee66ecc 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 6e92626fc93..11870318ce0 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 b9246f16c0a..dbd6090e49e 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 7831f533bc3..9b9ff390ecf 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 0f7c52bd3bb..0fd2ac0d23c 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 48c8bd24176..63dbdd48394 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 9f2b9ad3ff4..ba0725850f7 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 5bf57af203f..cba47a2f882 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 4e708a9e396..38ac70767b6 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 f59f2f77381..2ed96f28e67 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 615d8ce78ac..f718c363cd5 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 172572b73ad..a8f210f94e6 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 4ebb396c7d4..677e56c8902 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 01c529b923b..9dd6c118eab 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 2e6a02f6483..8ae83228a04 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 07c4a1628bc..00000000000 --- 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 555e2b5415a..00000000000 --- 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 5c11eaf791f..00000000000 --- 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 a94e98fbbef..00000000000 --- 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 977fcb99d4c..00000000000 --- 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 2d74530b436..844902dd100 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 ac089abfba3..00000000000 --- 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 72bf695fad3..00000000000 --- 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 fd720fd24fd..3932748e7e4 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 067877e5484..90068f463f7 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 653c0885cae..4d5f3c717b4 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 729530d82b7..25a10e3bd8f 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 857278b8416..96b3f1bc5f2 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 1294ee0a148..8d882d6ea77 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 83c0619c522..14b310d0db4 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 61e4679cdfb..6fdc74af28a 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 491ee4ecb76..5cdae049f1c 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 a987b73763c..e5c32f78da2 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 51dab28aa48..00000000000 --- 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 44497477362..7e4e473cf29 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 f0e5f2832bf..381df229bb3 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 5e0a9e990c0..824c9ce4e72 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 305ff7b75e1..db977f78a9f 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 85258fe2306..b41d116ef03 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 a7ba7c62815..8700c1d65f5 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 4fe85bfb05a..0c5671ea3e5 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 33c9af4085c..dd2be79bd22 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 3c5d2032892..86359395ef9 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 eb2a79fab7d..bfa912dbd76 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 6085896cfe3..4238fa03fb9 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 120a592e71a..9ec0c988fcb 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 6fcdf170dbf..f1f5085628e 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 ed091d614b7..92d596b2538 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 2b2a6ff96b1..36644e4da2d 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 59bc9b0fda9..47d4b0577ab 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 307f4385d90..ce17630e75c 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 7fb66af57ad..0300bad3a82 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 f6c056fdd4f..685b16a6726 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 eca1a65d813..ad39379d29b 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 277edb60834..9d715aeaa68 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 c0b088c03ce..bb9473ad0c5 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 00517be8733..aa22c4c2c9e 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 8a6b2440ef5..7dd105d0b39 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 93842a507e8..057cbb22ed6 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 8119c614f9d..8522266250f 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 87d6024c083..c14f0dae283 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 167441d3945..bf26f43f506 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 3e922c515ef..45be0f96b4c 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 17def7f4b6d..74b70ffe977 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 0b67699b733..2969ad347c8 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 e96d215fcf7..858c5915c9c 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 a4b681828fc..98845fcfb55 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.9 + 1.2.10 ROS2 Navigation Stack