From 0ec30eef8702f3fa049cc119d16b8f7e6cab11d6 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 8 Nov 2024 23:30:36 +0100 Subject: [PATCH] Humble sync 13: Nov 8, 2024 (#4748) * [DWB] Option to limit velocity commands in trajectory generator (#4663) * Option to limit vel cmd through traj generator Signed-off-by: huiyulhy * Cleanup Signed-off-by: huiyulhy * fix linting Signed-off-by: huiyulhy * Update linting Signed-off-by: huiyulhy * uncrustify Signed-off-by: huiyulhy * uncrustify Signed-off-by: huiyulhy --------- Signed-off-by: huiyulhy * fix to bt action server logging before bt execution result being ready (#4677) Signed-off-by: DreamWest * fix(simple-action-server): info log instead of warn on cancel (#4684) Cancelling a goal is nominal behavior and therefore it should not log warning. Signed-off-by: Rein Appeldoorn * [RotationShimController] fix: rotate to goal heading (#4724) Add frame_id to goal when rotating towards goal heading, otherwise the transform would fail. This bug was introduced in 30e2cde by not setting the frame_id. Signed-off-by: agennart Co-authored-by: agennart * Fix incorrect doxygen comment (#4741) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> * [map_io] Replace std logs by rclcpp logs (#4720) * replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * bump to 1.1.17 for humble sync Signed-off-by: Steve Macenski --------- Signed-off-by: huiyulhy Signed-off-by: DreamWest Signed-off-by: Rein Appeldoorn Signed-off-by: agennart Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Guillaume Doisy Signed-off-by: Steve Macenski Co-authored-by: Huiyu Leong <26198479+huiyulhy@users.noreply.github.com> Co-authored-by: DreamWest Co-authored-by: Rein Appeldoorn Co-authored-by: Saitama Co-authored-by: agennart Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- nav2_amcl/package.xml | 2 +- .../bt_action_server_impl.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_core/package.xml | 2 +- .../include/nav2_costmap_2d/costmap_2d.hpp | 3 +- nav2_costmap_2d/package.xml | 2 +- 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/standard_traj_generator.hpp | 3 + nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../src/standard_traj_generator.cpp | 10 ++ .../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_graceful_controller/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_map_server/src/map_io.cpp | 150 ++++++++++-------- nav2_mppi_controller/package.xml | 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 +- .../src/nav2_rotation_shim_controller.cpp | 1 + nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smoother/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- .../nav2_util/simple_action_server.hpp | 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 +- 46 files changed, 145 insertions(+), 108 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 9964578d41d..4f781b88841 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.16 + 1.1.17

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index e640251e1a5..4836267c58f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -298,18 +298,18 @@ void BtActionServer::executeCallback() switch (rc) { case nav2_behavior_tree::BtStatus::SUCCEEDED: - RCLCPP_INFO(logger_, "Goal succeeded"); action_server_->succeeded_current(result); + RCLCPP_INFO(logger_, "Goal succeeded"); break; case nav2_behavior_tree::BtStatus::FAILED: - RCLCPP_ERROR(logger_, "Goal failed"); action_server_->terminate_current(result); + RCLCPP_ERROR(logger_, "Goal failed"); break; case nav2_behavior_tree::BtStatus::CANCELED: - RCLCPP_INFO(logger_, "Goal canceled"); action_server_->terminate_all(result); + RCLCPP_INFO(logger_, "Goal canceled"); break; } } diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 3cbef2156c4..775a9c6c568 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.16 + 1.1.17 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index e6f70dec4cd..7be14f3e4a4 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.16 + 1.1.17 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 3f497d77e09..b4ccdcf7766 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.16 + 1.1.17 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 2fba14b6664..097aad40fef 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.16 + 1.1.17 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 79bb4eeb901..27b6171c087 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.16 + 1.1.17 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index b8b16cef0d2..4284710e4db 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.16 + 1.1.17 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 d9eaeda3465..f04608a76c1 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.16 + 1.1.17 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 6f909649d5d..ede94ad2a89 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.16 + 1.1.17 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 88c0e90f468..8e47589a7b4 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.16 + 1.1.17 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/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 04b9f4daa43..bac2fe6d12b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -150,8 +150,7 @@ class Costmap2D /** * @brief Get the cost of a cell in the costmap - * @param mx The x coordinate of the cell - * @param my The y coordinate of the cell + * @param index The cell index * @return The cost of the cell */ unsigned char getCost(unsigned int index) const; diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index d4d1a32acd2..fe18586310d 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.16 + 1.1.17 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_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index a2f4254bb60..1e8563b41e6 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.16 + 1.1.17 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 848aae5e260..8840c744cbd 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.16 + 1.1.17 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 01c7f80baf6..cd42c318870 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.16 + 1.1.17 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 a7e740b4c5f..4b02232c16f 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.16 + 1.1.17 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp index 87ba77baf02..db5467cd48d 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp @@ -147,6 +147,9 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator /// @brief the name of the overlying plugin ID std::string plugin_name_; + /// @brief Option to limit velocity in the trajectory generator by using current velocity + bool limit_vel_cmd_in_traj_; + /* Backwards Compatibility Parameter: include_last_point * * dwa had an off-by-one error built into it. diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 06b078c4ae4..facebd186ae 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.16 + 1.1.17 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index 49a62de302e..3b57df08e1e 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -75,6 +75,10 @@ void StandardTrajectoryGenerator::initialize( nh, plugin_name + ".include_last_point", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + nh, + plugin_name + ".limit_vel_cmd_in_traj", rclcpp::ParameterValue(false)); + /* * If discretize_by_time, then sim_granularity represents the amount of time that should be between * two successive points on the trajectory. @@ -89,6 +93,7 @@ void StandardTrajectoryGenerator::initialize( nh->get_parameter(plugin_name + ".linear_granularity", linear_granularity_); nh->get_parameter(plugin_name + ".angular_granularity", angular_granularity_); nh->get_parameter(plugin_name + ".include_last_point", include_last_point_); + nh->get_parameter(plugin_name + ".limit_vel_cmd_in_traj", limit_vel_cmd_in_traj_); } void StandardTrajectoryGenerator::initializeIterator( @@ -156,9 +161,14 @@ dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory( double running_time = 0.0; std::vector steps = getTimeSteps(cmd_vel); traj.poses.push_back(start_pose); + bool first_vel = false; for (double dt : steps) { // calculate velocities vel = computeNewVelocity(cmd_vel, vel, dt); + if (!first_vel && limit_vel_cmd_in_traj_) { + traj.velocity = vel; + first_vel = true; + } // update the position of the robot using the velocities passed in pose = computeNewPosition(pose, vel, dt); diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index dea815479d3..596885d6144 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.1.16 + 1.1.17 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 1f983a54f26..09ef1a25a13 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.1.16 + 1.1.17 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 54a58337cea..4e374ab6e2f 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.1.16 + 1.1.17 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index a66797726dc..9b41c451bae 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.1.16 + 1.1.17 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 02c71c3112b..c92559d835b 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.16 + 1.1.17 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 67f94ef8c67..78575241474 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.16 + 1.1.17 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 85428490edb..23f0f55b07a 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -132,8 +132,10 @@ std::string expand_user_home_dir_if_needed( 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"; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "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}; @@ -181,15 +183,18 @@ LoadParameters loadMapYaml(const std::string & yaml_filename) load_parameters.negate = yaml_get_value(doc, "negate"); } - std::cout << "[DEBUG] [map_io]: resolution: " << load_parameters.resolution << std::endl; - std::cout << "[DEBUG] [map_io]: origin[0]: " << load_parameters.origin[0] << std::endl; - std::cout << "[DEBUG] [map_io]: origin[1]: " << load_parameters.origin[1] << std::endl; - std::cout << "[DEBUG] [map_io]: origin[2]: " << load_parameters.origin[2] << std::endl; - std::cout << "[DEBUG] [map_io]: free_thresh: " << load_parameters.free_thresh << std::endl; - std::cout << "[DEBUG] [map_io]: occupied_thresh: " << load_parameters.occupied_thresh << - std::endl; - std::cout << "[DEBUG] [map_io]: mode: " << map_mode_to_string(load_parameters.mode) << std::endl; - std::cout << "[DEBUG] [map_io]: negate: " << load_parameters.negate << std::endl; //NOLINT + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "resolution: " << load_parameters.resolution); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[0]: " << load_parameters.origin[0]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[1]: " << load_parameters.origin[1]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[2]: " << load_parameters.origin[2]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "free_thresh: " << load_parameters.free_thresh); + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "occupied_thresh: " << load_parameters.occupied_thresh); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), + "mode: " << map_mode_to_string(load_parameters.mode)); + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "negate: " << load_parameters.negate); return load_parameters; } @@ -201,8 +206,9 @@ void loadMapFromFile( Magick::InitializeMagick(nullptr); nav_msgs::msg::OccupancyGrid msg; - std::cout << "[INFO] [map_io]: Loading image_file: " << - load_parameters.image_file_name << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), "Loading image_file: " << + load_parameters.image_file_name); Magick::Image img(load_parameters.image_file_name); // Copy the image data into the map structure @@ -290,9 +296,11 @@ void loadMapFromFile( msg.header.frame_id = "map"; msg.header.stamp = clock.now(); - std::cout << - "[DEBUG] [map_io]: Read map " << load_parameters.image_file_name << ": " << msg.info.width << - " X " << msg.info.height << " map @ " << msg.info.resolution << " m/cell" << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Read map " << load_parameters.image_file_name + << ": " << msg.info.width << " X " << msg.info.height << " map @ " + << msg.info.resolution << " m/cell"); map = msg; } @@ -302,30 +310,32 @@ LOAD_MAP_STATUS loadMapFromYaml( nav_msgs::msg::OccupancyGrid & map) { if (yaml_file.empty()) { - std::cerr << "[ERROR] [map_io]: YAML file name is empty, can't load!" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "YAML file name is empty, can't load!"); return MAP_DOES_NOT_EXIST; } - std::cout << "[INFO] [map_io]: Loading yaml file: " << yaml_file << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Loading yaml file: " << yaml_file); LoadParameters load_parameters; try { load_parameters = loadMapYaml(yaml_file); } catch (YAML::Exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed processing YAML file " << yaml_file << " at position (" << - e.mark.line << ":" << e.mark.column << ") for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Failed processing YAML file " << yaml_file << " at position (" << + e.mark.line << ":" << e.mark.column << ") for reason: " << e.what()); return INVALID_MAP_METADATA; } catch (std::exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed to parse map YAML loaded from file " << yaml_file << - " for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), "Failed to parse map YAML loaded from file " << yaml_file << + " for reason: " << e.what()); return INVALID_MAP_METADATA; } try { loadMapFromFile(load_parameters, map); } catch (std::exception & e) { - std::cerr << - "[ERROR] [map_io]: Failed to load image file " << load_parameters.image_file_name << - " for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Failed to load image file " << load_parameters.image_file_name << + " for reason: " << e.what()); return INVALID_MAP_DATA; } @@ -350,40 +360,46 @@ void checkSaveParameters(SaveParameters & save_parameters) rclcpp::Clock clock(RCL_SYSTEM_TIME); save_parameters.map_file_name = "map_" + std::to_string(static_cast(clock.now().seconds())); - std::cout << "[WARN] [map_io]: Map file unspecified. Map will be saved to " << - save_parameters.map_file_name << " file" << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Map file unspecified. Map will be saved to " << + save_parameters.map_file_name << " file"); } // Checking thresholds if (save_parameters.occupied_thresh == 0.0) { save_parameters.occupied_thresh = 0.65; - std::cout << "[WARN] [map_io]: Occupied threshold unspecified. Setting it to default value: " << - save_parameters.occupied_thresh << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger( + "map_io"), "Occupied threshold unspecified. Setting it to default value: " << + save_parameters.occupied_thresh); } if (save_parameters.free_thresh == 0.0) { save_parameters.free_thresh = 0.25; - std::cout << "[WARN] [map_io]: Free threshold unspecified. Setting it to default value: " << - save_parameters.free_thresh << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Free threshold unspecified. Setting it to default value: " << + save_parameters.free_thresh); } if (1.0 < save_parameters.occupied_thresh) { - std::cerr << "[ERROR] [map_io]: Threshold_occupied must be 1.0 or less" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Threshold_occupied must be 1.0 or less"); throw std::runtime_error("Incorrect thresholds"); } if (save_parameters.free_thresh < 0.0) { - std::cerr << "[ERROR] [map_io]: Free threshold must be 0.0 or greater" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Free threshold must be 0.0 or greater"); throw std::runtime_error("Incorrect thresholds"); } if (save_parameters.occupied_thresh <= save_parameters.free_thresh) { - std::cerr << "[ERROR] [map_io]: Threshold_free must be smaller than threshold_occupied" << - std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Threshold_free must be smaller than threshold_occupied"); throw std::runtime_error("Incorrect thresholds"); } // Checking image format if (save_parameters.image_format == "") { save_parameters.image_format = save_parameters.mode == MapMode::Scale ? "png" : "pgm"; - std::cout << "[WARN] [map_io]: Image format unspecified. Setting it to: " << - save_parameters.image_format << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Image format unspecified. Setting it to: " << + save_parameters.image_format); } std::transform( @@ -406,24 +422,25 @@ void checkSaveParameters(SaveParameters & save_parameters) ss << "'" << format_name << "'"; first = false; } - std::cout << - "[WARN] [map_io]: Requested image format '" << save_parameters.image_format << - "' is not one of the recommended formats: " << ss.str() << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Requested image format '" << save_parameters.image_format << + "' is not one of the recommended formats: " << ss.str()); } const std::string FALLBACK_FORMAT = "png"; try { Magick::CoderInfo info(save_parameters.image_format); if (!info.isWritable()) { - std::cout << - "[WARN] [map_io]: Format '" << save_parameters.image_format << - "' is not writable. Using '" << FALLBACK_FORMAT << "' instead" << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Format '" << save_parameters.image_format << + "' is not writable. Using '" << FALLBACK_FORMAT << "' instead"); save_parameters.image_format = FALLBACK_FORMAT; } } catch (Magick::ErrorOption & e) { - std::cout << - "[WARN] [map_io]: Format '" << save_parameters.image_format << "' is not usable. Using '" << - FALLBACK_FORMAT << "' instead:" << std::endl << e.what() << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger( + "map_io"), "Format '" << save_parameters.image_format << "' is not usable. Using '" << + FALLBACK_FORMAT << "' instead:" << std::endl << e.what()); save_parameters.image_format = FALLBACK_FORMAT; } @@ -434,10 +451,10 @@ void checkSaveParameters(SaveParameters & save_parameters) save_parameters.image_format == "jpg" || save_parameters.image_format == "jpeg")) { - std::cout << - "[WARN] [map_io]: Map mode 'scale' requires transparency, but format '" << - save_parameters.image_format << - "' does not support it. Consider switching image format to 'png'." << std::endl; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("map_io"), "Map mode 'scale' requires transparency, but format '" << + save_parameters.image_format << + "' does not support it. Consider switching image format to 'png'."); } } @@ -451,9 +468,10 @@ void tryWriteMapToFile( const nav_msgs::msg::OccupancyGrid & map, const SaveParameters & save_parameters) { - std::cout << - "[INFO] [map_io]: Received a " << map.info.width << " X " << map.info.height << " map @ " << - map.info.resolution << " m/pix" << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger( + "map_io"), "Received a " << map.info.width << " X " << map.info.height << " map @ " << + map.info.resolution << " m/pix"); std::string mapdatafile = save_parameters.map_file_name + "." + save_parameters.image_format; { @@ -509,14 +527,18 @@ void tryWriteMapToFile( pixel = Magick::Color(q, q, q); break; default: - std::cerr << "[ERROR] [map_io]: Map mode should be Trinary, Scale or Raw" << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "map_io"), "Map mode should be Trinary, Scale or Raw"); throw std::runtime_error("Invalid map mode"); } image.pixelColor(x, y, pixel); } } - std::cout << "[INFO] [map_io]: Writing map occupancy data to " << mapdatafile << std::endl; + RCLCPP_INFO_STREAM( + rclcpp::get_logger("map_io"), + "Writing map occupancy data to " << mapdatafile); image.write(mapdatafile); } @@ -545,15 +567,15 @@ void tryWriteMapToFile( e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh; if (!e.good()) { - std::cout << - "[WARN] [map_io]: YAML writer failed with an error " << e.GetLastError() << - ". The map metadata may be invalid." << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), "YAML writer failed with an error " << e.GetLastError() << + ". The map metadata may be invalid."); } - std::cout << "[INFO] [map_io]: Writing map metadata to " << mapmetadatafile << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Writing map metadata to " << mapmetadatafile); std::ofstream(mapmetadatafile) << e.c_str(); } - std::cout << "[INFO] [map_io]: Map saved" << std::endl; + RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Map saved"); } bool saveMapToFile( @@ -569,7 +591,9 @@ bool saveMapToFile( tryWriteMapToFile(map, save_parameters_loc); } catch (std::exception & e) { - std::cout << "[ERROR] [map_io]: Failed to write map for reason: " << e.what() << std::endl; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("map_io"), + "Failed to write map for reason: " << e.what()); return false; } return true; diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 82d37bb8b68..da7fbf07cb3 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.16 + 1.1.17 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index f024425756d..02c844a673c 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.16 + 1.1.17 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 4dec58b4705..e151b8d16ff 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.16 + 1.1.17 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 884eb2c33dd..3444a73c61e 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.16 + 1.1.17 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 3ca8d52e075..0beebae9a1a 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.1.16 + 1.1.17 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 130c589fded..653761ae527 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.16 + 1.1.17 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 8a2ede557ee..28fdc37aea2 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -258,6 +258,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() } auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; goal.header.stamp = clock_->now(); return goal; } diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index b903d1d2c8c..abedb8e1c85 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.16 + 1.1.17 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 c3064d3b24d..eeb9b3a4068 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.16 + 1.1.17 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 10923a7f254..b9e4e32135a 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.16 + 1.1.17 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 42e993c2b28..749ba596e0c 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.16 + 1.1.17 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index c0b779291ca..3181d288da1 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.16 + 1.1.17 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index db5092cb240..aa9507cb6b7 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.16 + 1.1.17 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 391369e300b..887df52c5aa 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -557,7 +557,7 @@ class SimpleActionServer if (is_active(handle)) { if (handle->is_canceling()) { - warn_msg("Client requested to cancel the goal. Cancelling."); + info_msg("Client requested to cancel the goal. Cancelling."); handle->canceled(result); } else { warn_msg("Aborting handle."); diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 8102306c237..a806923a6f1 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.16 + 1.1.17 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index dabe9d46c5a..1844691fc92 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.16 + 1.1.17 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 c5f568b2bfb..e9fc6cbd5d0 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.16 + 1.1.17 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 f4a2e2cb222..bb6dbdf723c 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.16 + 1.1.17 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index b2b00936db2..9313fa61360 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.16 + 1.1.17 ROS2 Navigation Stack