Nav2 Sync 5 2024 Nov 11 (#31)

* Adding disengagement threshold to rotation shim controller (backport ros-navigation#4699) (ros-navigation#4702)

* Adding disengagement threshold to rotation shim controller (ros-navigation#4699)

* adding disengagement threshold to rotation shim controller

Signed-off-by: Steve Macenski <[email protected]>

* change default to 22.5 deg

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>
(cherry picked from commit fc7e086)

* Update nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

* fix(nav2_theta_star_planner) Fix crash on Humble when goal is outside map bounds (ros-navigation#4706)

* Humble sync 13: Nov 8, 2024 (ros-navigation#4748)

* [DWB] Option to limit velocity commands in trajectory generator (ros-navigation#4663)

* Option to limit vel cmd through traj generator

Signed-off-by: huiyulhy <[email protected]>

* Cleanup

Signed-off-by: huiyulhy <[email protected]>

* fix linting

Signed-off-by: huiyulhy <[email protected]>

* Update linting

Signed-off-by: huiyulhy <[email protected]>

* uncrustify

Signed-off-by: huiyulhy <[email protected]>

* uncrustify

Signed-off-by: huiyulhy <[email protected]>

---------

Signed-off-by: huiyulhy <[email protected]>

* fix to bt action server logging before bt execution result being ready (ros-navigation#4677)

Signed-off-by: DreamWest <[email protected]>

* fix(simple-action-server): info log instead of warn on cancel (ros-navigation#4684)

Cancelling a goal is nominal behavior and therefore it should not log
warning.

Signed-off-by: Rein Appeldoorn <[email protected]>

* [RotationShimController] fix: rotate to goal heading (ros-navigation#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 <[email protected]>
Co-authored-by: agennart <[email protected]>

* Fix incorrect doxygen comment (ros-navigation#4741)

Signed-off-by: Ryan Friedman <[email protected]>

* [map_io] Replace std logs by rclcpp logs (ros-navigation#4720)

* replace std logs by rclcpp logs

Signed-off-by: Guillaume Doisy <[email protected]>

* RCLCPP_DEBUG to RCLCPP_INFO for visibility

Signed-off-by: Guillaume Doisy <[email protected]>

---------

Signed-off-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>

* bump to 1.1.17 for humble sync

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: huiyulhy <[email protected]>
Signed-off-by: DreamWest <[email protected]>
Signed-off-by: Rein Appeldoorn <[email protected]>
Signed-off-by: agennart <[email protected]>
Signed-off-by: Ryan Friedman <[email protected]>
Signed-off-by: Guillaume Doisy <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Co-authored-by: Huiyu Leong <[email protected]>
Co-authored-by: DreamWest <[email protected]>
Co-authored-by: Rein Appeldoorn <[email protected]>
Co-authored-by: Saitama <[email protected]>
Co-authored-by: agennart <[email protected]>
Co-authored-by: Ryan <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: huiyulhy <[email protected]>
Signed-off-by: DreamWest <[email protected]>
Signed-off-by: Rein Appeldoorn <[email protected]>
Signed-off-by: agennart <[email protected]>
Signed-off-by: Ryan Friedman <[email protected]>
Signed-off-by: Guillaume Doisy <[email protected]>
Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com>
Co-authored-by: Steve Macenski <[email protected]>
Co-authored-by: brayanpa <[email protected]>
Co-authored-by: Huiyu Leong <[email protected]>
Co-authored-by: DreamWest <[email protected]>
Co-authored-by: Rein Appeldoorn <[email protected]>
Co-authored-by: Saitama <[email protected]>
Co-authored-by: agennart <[email protected]>
Co-authored-by: Ryan <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>

nav2_amcl/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -298,18 +298,18 @@ void BtActionServer<ActionT>::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;
   }
 }

nav2_behavior_tree/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>

nav2_behaviors/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

nav2_bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bringup</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

nav2_bt_navigator/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bt_navigator</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

nav2_collision_monitor/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_collision_monitor</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>Collision Monitor</description>
   <maintainer email="[email protected]">Alexey Merzlyakov</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

nav2_common/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_common</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_constrained_smoother/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_constrained_smoother</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="[email protected]">Matej Vargovcik</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

nav2_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_controller</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>Controller action interface</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_core/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_core</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">Carl Delsey</maintainer>

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;

nav2_costmap_2d/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format ="3">
   <name>nav2_costmap_2d</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>
     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

nav2_dwb_controller/costmap_queue/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>The costmap_queue package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_core/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_core</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_critics/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>The dwb_critics package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_msgs</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

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.

nav2_dwb_controller/dwb_plugins/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_plugins</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>
       Standard implementations of the GoalChecker
       and TrajectoryGenerators for dwb_core

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<double> 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);

nav2_dwb_controller/nav2_dwb_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_dwb_controller</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>
     ROS2 controller (DWB) metapackage
   </description>

nav2_dwb_controller/nav_2d_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav_2d_msgs</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D.</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/nav_2d_utils/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav_2d_utils</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>A handful of useful utility functions for nav_2d packages.</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_graceful_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_graceful_controller</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>Graceful motion controller</description>
   <maintainer email="[email protected]">Alberto Tudela</maintainer>
   <license>Apache-2.0</license>

nav2_lifecycle_manager/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_lifecycle_manager</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>A controller/manager for the lifecycle nodes of the Navigation 2 system</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

nav2_map_server/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_map_server</name>
-  <version>1.1.16</version>
+  <version>1.1.17</version>
   <description>
     Refactored map server for ROS2 Navigation
   </description>