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>
ros-navigation · Aug 24, 2020 · fa1a64a · fa1a64a
1 parent db2f917
commit fa1a64a
Show file tree

Hide file tree

Showing 35 changed files with 130 additions and 93 deletions.
diff --git a/nav2_amcl/package.xml b/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

diff --git a/nav2_behavior_tree/package.xml b/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>

diff --git a/nav2_bringup/bringup/package.xml b/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>

diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/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>

diff --git a/nav2_bt_navigator/package.xml b/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>

diff --git a/nav2_common/package.xml b/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>

diff --git a/nav2_controller/package.xml b/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>

diff --git a/nav2_controller/src/nav2_controller.cpp b/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(

diff --git a/nav2_core/package.xml b/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>

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/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

diff --git a/nav2_costmap_2d/package.xml b/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>0.4.2</version>
+  <version>0.4.3</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

diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -71,6 +71,12 @@ InflationLayer::InflationLayer()
   last_max_x_(std::numeric_limits<double>::max()),
   last_max_y_(std::numeric_limits<double>::max())
 {
+  access_ = new mutex_t();
+}
+
+InflationLayer::~InflationLayer()
+{
+  delete access_;
 }
 
 void
@@ -160,6 +166,7 @@ InflationLayer::updateCosts(
   int max_i,
   int max_j)
 {
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   if (!enabled_ || (cell_inflation_radius_ == 0)) {
     return;
   }
@@ -305,6 +312,7 @@ InflationLayer::enqueue(
 void
 InflationLayer::computeCaches()
 {
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   if (cell_inflation_radius_ == 0) {
     return;
   }

diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>0.4.2</version>
+  <version>0.4.3</version>
   <description>The costmap_queue package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_core/package.xml b/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>0.4.2</version>
+  <version>0.4.3</version>
   <description>TODO</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>0.4.2</version>
+  <version>0.4.3</version>
   <description>The dwb_critics package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/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>0.4.2</version>
+  <version>0.4.3</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_plugins</name>
-  <version>0.4.2</version>
+  <version>0.4.3</version>
   <description>
       Standard implementations of the GoalChecker
       and TrajectoryGenerators for dwb_core