Foxy sync 3 (#1965)

* waypoint_follower node has _rclcpp_node as well as base node (#1940)

* Add #include <vector> for vector<> (#1946)

To fix cpplint

* Add 'angles' dependency to nav2_costmap_2d package.xml (#1947)

* transform goal to costmap frame (#1949)

The plan recieved is usually in global frame, but
our local costmap is often in odom frame. This
fixes a regression from #1857

* Add mutex lock into inflation layer to avoid thread issue in updating footprint (#1952)

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* Fix being unable to change StandardTrajectoryGenerator parameter vtheta_samples (#1619)

* Fix tests declaring parameters real nodes don't

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Fix loadParameterWithDeprecation not getting initial parameter values

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Create sim_time variable before using it

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Line length < 100

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add missing {

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Linter fixes

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* sim_granularity -> time_granularity

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Linter fix

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* update version to 0.4.3

* removing redundant dep on angles

Co-authored-by: Ruffin <roxfoxpox@gmail.com>
Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
Co-authored-by: Michael Ferguson <mfergs7@gmail.com>
Co-authored-by: Daisuke Sato <43101027+daisukes@users.noreply.github.com>
Co-authored-by: Shane Loretz <sloretz@openrobotics.org>

Author: 6 people

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>0.4.2</version>
+  <version>0.4.3</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

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>0.4.2</version>
+  <version>0.4.3</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

nav2_bringup/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>0.4.2</version>
+  <version>0.4.3</version>
   <description>Bringup scripts and configurations for the navigation2 stack</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_bringup/nav2_gazebo_spawner/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_gazebo_spawner</name>
-  <version>0.4.2</version>
+  <version>0.4.3</version>
   <description>Package for spawning a robot model into Gazebo for navigation2</description>
   <maintainer email="carlos.orduno@intel.com">lkumarbe</maintainer>
   <maintainer email="lalit.kumar.begani@intel.com">lkumarbe</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>0.4.2</version>
+  <version>0.4.3</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

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>0.4.2</version>
+  <version>0.4.3</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

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>0.4.2</version>
+  <version>0.4.3</version>
   <description>Controller action interface</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_controller/src/nav2_controller.cpp

@@ -20,6 +20,7 @@
 
 #include "nav2_core/exceptions.hpp"
 #include "nav_2d_utils/conversions.hpp"
+#include "nav_2d_utils/tf_help.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_controller/nav2_controller.hpp"
@@ -328,6 +329,11 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
   controllers_[current_controller_]->setPlan(path);
 
   auto end_pose = path.poses.back();
+  end_pose.header.frame_id = path.header.frame_id;
+  rclcpp::Duration tolerance(costmap_ros_->getTransformTolerance() * 1e9);
+  nav_2d_utils::transformPose(
+    costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(),
+    end_pose, end_pose, tolerance);
   goal_checker_->reset();
 
   RCLCPP_DEBUG(

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>0.4.2</version>
+  <version>0.4.3</version>
   <description>A set of headers for plugins core to the navigation2 stack</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -40,6 +40,7 @@
 
 #include <map>
 #include <vector>
+#include <mutex>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/layer.hpp"
@@ -77,7 +78,7 @@ class InflationLayer : public Layer
 public:
   InflationLayer();
 
-  ~InflationLayer() override = default;
+  ~InflationLayer();
 
   void onInitialize() override;
   void updateBounds(
@@ -115,6 +116,13 @@ class InflationLayer : public Layer
     return cost;
   }
 
+  // Provide a typedef to ease future code maintenance
+  typedef std::recursive_mutex mutex_t;
+  mutex_t * getMutex()
+  {
+    return access_;
+  }
+
 protected:
   void onFootprintChanged() override;
 
@@ -184,6 +192,7 @@ class InflationLayer : public Layer
 
   // Indicates that the entire costmap should be reinflated next time around.
   bool need_reinflation_;
+  mutex_t * access_;
 };
 
 }  // namespace nav2_costmap_2d