Merge branch 'ros2-master' into galactic-devel

Tony Najjar · web-flow · commit df4c61218d9e · 2022-10-19T18:00:27.000+02:00
diff --git a/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h b/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h
@@ -125,7 +125,7 @@ class HomotopyClassPlanner : public PlannerInterface
    * @param visualization Shared pointer to the TebVisualization class (optional)
    * @param via_points Container storing via-points (optional)
    */
-  HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = std::make_shared<PointRobotFootprint>(),
+  HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
                        TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
 
   /**
@@ -138,16 +138,12 @@ class HomotopyClassPlanner : public PlannerInterface
    * @param node Shared pointer for rclcpp::Node
    * @param cfg Const reference to the TebConfig class for internal parameters
    * @param obstacles Container storing all relevant obstacles (see Obstacle)
-   * @param robot_model Shared pointer to the robot shape model used for optimization (optional)
    * @param visualization Shared pointer to the TebVisualization class (optional)
    * @param via_points Container storing via-points (optional)
    */
-  void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = std::make_shared<PointRobotFootprint>(),
+  void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
                   TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
 
-
-  void updateRobotModel(RobotFootprintModelPtr robot_model );
-
   /** @name Plan a trajectory */
   //@{
 
diff --git a/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp b/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp
@@ -65,7 +65,7 @@ EquivalenceClassPtr HomotopyClassPlanner::calculateEquivalenceClass(BidirIter pa
 template<typename BidirIter, typename Fun>
 TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
 {
-  TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, robot_model_));
+  TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_));
 
   candidate->teb().initTrajectoryToGoal(path_start, path_end, fun_position, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
                                  cfg_->robot.acc_lim_x, cfg_->robot.acc_lim_theta, start_orientation, goal_orientation, cfg_->trajectory.min_samples,
diff --git a/teb_local_planner/include/teb_local_planner/optimal_planner.h b/teb_local_planner/include/teb_local_planner/optimal_planner.h
@@ -125,11 +125,10 @@ class TebOptimalPlanner : public PlannerInterface
    * @brief Construct and initialize the TEB optimal planner.
    * @param cfg Const reference to the TebConfig class for internal parameters
    * @param obstacles Container storing all relevant obstacles (see Obstacle)
-   * @param robot_model Shared pointer to the robot shape model used for optimization (optional)
    * @param visual Shared pointer to the TebVisualization class (optional)
    * @param via_points Container storing via-points (optional)
    */
-  TebOptimalPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = std::make_shared<PointRobotFootprint>(),
+  TebOptimalPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
                     TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
   
   /**
@@ -146,14 +145,9 @@ class TebOptimalPlanner : public PlannerInterface
     * @param visual Shared pointer to the TebVisualization class (optional)
     * @param via_points Container storing via-points (optional)
     */
-  void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = std::make_shared<PointRobotFootprint>(),
+  void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
                   TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
   
-  /**
-    * @param robot_model Shared pointer to the robot shape model used for optimization (optional)
-    */
-  void updateRobotModel(RobotFootprintModelPtr robot_model );
-  
   /** @name Plan a trajectory  */
   //@{
   
diff --git a/teb_local_planner/include/teb_local_planner/planner_interface.h b/teb_local_planner/include/teb_local_planner/planner_interface.h
@@ -173,10 +173,6 @@ class PlannerInterface
   }
 
   virtual void setVisualization(const TebVisualizationPtr & visualization) = 0;
-  
-  virtual void updateRobotModel(RobotFootprintModelPtr robot_model)
-  {
-  }
 
   /**
    * @brief Check whether the planned trajectory is feasible or not.
diff --git a/teb_local_planner/include/teb_local_planner/pose_se2.h b/teb_local_planner/include/teb_local_planner/pose_se2.h
@@ -48,7 +48,7 @@
 
 #include <tf2/convert.h>
 #include <tf2/utils.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 namespace teb_local_planner
 {
diff --git a/teb_local_planner/include/teb_local_planner/teb_config.h b/teb_local_planner/include/teb_local_planner/teb_config.h
@@ -45,6 +45,8 @@
 #include <Eigen/Core>
 #include <Eigen/StdVector>
 #include <nav_2d_utils/parameters.hpp>
+#include "teb_local_planner/robot_footprint_model.h"
+#include <nav2_costmap_2d/footprint.hpp>
 
 // Definitions
 #define USE_ANALYTIC_JACOBI // if available for a specific edge, use analytic jacobi
@@ -64,6 +66,13 @@ class TebConfig
   std::string map_frame; //!< Global planning frame
   std::string node_name; //!< node name used for parameter event callback
 
+  RobotFootprintModelPtr robot_model;
+  std::string model_name;
+  double radius;
+  std::vector<double> line_start, line_end;
+  double front_offset, front_radius, rear_offset, rear_radius;
+  std::string footprint_string;
+
   //! Trajectory related parameters
   struct Trajectory
   {
@@ -393,11 +402,11 @@ class TebConfig
   void loadRosParamFromNodeHandle(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name);
   
   /**
-   * @brief Paremeter event callback
-   * @param event The ParameterEvent
+   * @brief Callback executed when a paramter change is detected
+   * @param parameters list of changed parameters
    */
-  void on_parameter_event_callback(
-      const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
+  rcl_interfaces::msg::SetParametersResult
+    dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
   
   /**
    * @brief Check parameters and print warnings in case of discrepancies
diff --git a/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h b/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h
@@ -71,6 +71,7 @@
 #include <nav2_util/lifecycle_node.hpp>
 #include <nav2_costmap_2d/costmap_2d_ros.hpp>
 #include <nav_2d_utils/parameters.hpp>
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
 // dynamic reconfigure
 //#include "teb_local_planner/TebLocalPlannerReconfigureConfig.h>
 //#include <dynamic_reconfigure/server.h>
@@ -110,8 +111,8 @@ class TebLocalPlannerROS : public nav2_core::Controller
   void configure(
     const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
     std::string name,
-    const std::shared_ptr<tf2_ros::Buffer> & tf,
-    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) override;
+    std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
   void activate() override;
   void deactivate() override;
   void cleanup() override;
@@ -406,18 +407,14 @@ class TebLocalPlannerROS : public nav2_core::Controller
   std::vector<geometry_msgs::msg::Point> footprint_spec_; //!< Store the footprint of the robot 
   double robot_inscribed_radius_; //!< The radius of the inscribed circle of the robot (collision possible)
   double robot_circumscribed_radius; //!< The radius of the circumscribed circle of the robot
-  
-  std::string global_frame_; //!< The frame in which the controller will run
-  std::string robot_base_frame_; //!< Used as the base frame id of the robot
     
   // flags
   bool initialized_; //!< Keeps track about the correct initialization of this class
   std::string name_; //!< Name of plugin ID
 
-  // Subscription for parameter change
-  rclcpp::AsyncParametersClient::SharedPtr parameters_client_;
-  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
-
+protected:
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
diff --git a/teb_local_planner/src/homotopy_class_planner.cpp b/teb_local_planner/src/homotopy_class_planner.cpp
@@ -47,24 +47,23 @@ HomotopyClassPlanner::HomotopyClassPlanner() : cfg_(NULL), obstacles_(NULL), via
 {
 }
 
-HomotopyClassPlanner::HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model,
+HomotopyClassPlanner::HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles,
                                            TebVisualizationPtr visual, const ViaPointContainer* via_points) : initial_plan_(NULL)
 {
-  initialize(node, cfg, obstacles, robot_model, visual, via_points);
+  initialize(node, cfg, obstacles, visual, via_points);
 }
 
 HomotopyClassPlanner::~HomotopyClassPlanner()
 {
 }
 
-void HomotopyClassPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model,
+void HomotopyClassPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles,
                                       TebVisualizationPtr visual, const ViaPointContainer* via_points)
 {
   node_ = node;
   cfg_ = &cfg;
   obstacles_ = obstacles;
   via_points_ = via_points;
-  robot_model_ = robot_model;
 
   if (cfg_->hcp.simple_exploration)
     graph_search_ = std::shared_ptr<GraphSearchInterface>(new lrKeyPointGraph(*cfg_, this));
@@ -82,11 +81,6 @@ void HomotopyClassPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node,
   setVisualization(visual);
 }
 
-void HomotopyClassPlanner::updateRobotModel(RobotFootprintModelPtr robot_model )
-{
-  robot_model_ = robot_model;
-}
-
 void HomotopyClassPlanner::setVisualization(const TebVisualizationPtr& visualization)
 {
   visualization_ = visualization;
@@ -171,7 +165,7 @@ void HomotopyClassPlanner::visualize()
       visualization_->publishLocalPlanAndPoses(best_teb->teb());
 
       if (best_teb->teb().sizePoses() > 0) //TODO maybe store current pose (start) within plan method as class field.
-        visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *robot_model_);
+        visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *cfg_->robot_model);
 
       // feedback message
       if (cfg_->trajectory.publish_feedback)
@@ -372,7 +366,7 @@ TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const PoseSE2& start
 {
   if(tebs_.size() >= cfg_->hcp.max_number_classes)
     return TebOptimalPlannerPtr();
-  TebOptimalPlannerPtr candidate =  TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, robot_model_, visualization_));
+  TebOptimalPlannerPtr candidate =  TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, visualization_));
 
   candidate->teb().initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
 
@@ -427,7 +421,7 @@ TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const std::vector<ge
 {
   if(tebs_.size() >= cfg_->hcp.max_number_classes)
     return TebOptimalPlannerPtr();
-  TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, robot_model_, visualization_));
+  TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, visualization_));
 
   candidate->teb().initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
     cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
diff --git a/teb_local_planner/src/optimal_planner.cpp b/teb_local_planner/src/optimal_planner.cpp
diff --git a/teb_local_planner/src/teb_config.cpp b/teb_local_planner/src/teb_config.cpp
diff --git a/teb_local_planner/src/teb_local_planner_ros.cpp b/teb_local_planner/src/teb_local_planner_ros.cpp

Original file line number	Diff line number	Diff line change
`@@ -65,7 +65,7 @@ EquivalenceClassPtr HomotopyClassPlanner::calculateEquivalenceClass(BidirIter pa`
`65`	`65`	`template<typename BidirIter, typename Fun>`
`66`	`66`	`TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)`
`67`	`67`	`{`
`68`		`- TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, robot_model_));`
	`68`	`+ TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_));`
`69`	`69`
`70`	`70`	`candidate->teb().initTrajectoryToGoal(path_start, path_end, fun_position, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,`
`71`	`71`	`cfg_->robot.acc_lim_x, cfg_->robot.acc_lim_theta, start_orientation, goal_orientation, cfg_->trajectory.min_samples,`
Original file line number	Diff line number	Diff line change
`@@ -173,10 +173,6 @@ class PlannerInterface`
`173`	`173`	`}`
`174`	`174`
`175`	`175`	`virtual void setVisualization(const TebVisualizationPtr & visualization) = 0;`
`176`		`-`
`177`		`- virtual void updateRobotModel(RobotFootprintModelPtr robot_model)`
`178`		`- {`
`179`		`- }`
`180`	`176`
`181`	`177`	`/**`
`182`	`178`	`* @brief Check whether the planned trajectory is feasible or not.`
Original file line number	Diff line number	Diff line change
`@@ -48,7 +48,7 @@`
`48`	`48`
`49`	`49`	`#include <tf2/convert.h>`
`50`	`50`	`#include <tf2/utils.h>`
`51`		`-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>`
	`51`	`+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>`
`52`	`52`
`53`	`53`	`namespace teb_local_planner`
`54`	`54`	`{`
Original file line number	Diff line number	Diff line change
`@@ -47,24 +47,23 @@ HomotopyClassPlanner::HomotopyClassPlanner() : cfg_(NULL), obstacles_(NULL), via`
`47`	`47`	`{`
`48`	`48`	`}`
`49`	`49`
`50`		`-HomotopyClassPlanner::HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model,`
	`50`	`+HomotopyClassPlanner::HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles,`
`51`	`51`	`TebVisualizationPtr visual, const ViaPointContainer* via_points) : initial_plan_(NULL)`
`52`	`52`	`{`
`53`		`- initialize(node, cfg, obstacles, robot_model, visual, via_points);`
	`53`	`+ initialize(node, cfg, obstacles, visual, via_points);`
`54`	`54`	`}`
`55`	`55`
`56`	`56`	`HomotopyClassPlanner::~HomotopyClassPlanner()`
`57`	`57`	`{`
`58`	`58`	`}`
`59`	`59`
`60`		`-void HomotopyClassPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model,`
	`60`	`+void HomotopyClassPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles,`
`61`	`61`	`TebVisualizationPtr visual, const ViaPointContainer* via_points)`
`62`	`62`	`{`
`63`	`63`	`node_ = node;`
`64`	`64`	`cfg_ = &cfg;`
`65`	`65`	`obstacles_ = obstacles;`
`66`	`66`	`via_points_ = via_points;`
`67`		`- robot_model_ = robot_model;`
`68`	`67`
`69`	`68`	`if (cfg_->hcp.simple_exploration)`
`70`	`69`	`graph_search_ = std::shared_ptr<GraphSearchInterface>(new lrKeyPointGraph(*cfg_, this));`
`@@ -82,11 +81,6 @@ void HomotopyClassPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node,`
`82`	`81`	`setVisualization(visual);`
`83`	`82`	`}`
`84`	`83`
`85`		`-void HomotopyClassPlanner::updateRobotModel(RobotFootprintModelPtr robot_model )`
`86`		`-{`
`87`		`- robot_model_ = robot_model;`
`88`		`-}`
`89`		`-`
`90`	`84`	`void HomotopyClassPlanner::setVisualization(const TebVisualizationPtr& visualization)`
`91`	`85`	`{`
`92`	`86`	`visualization_ = visualization;`
`@@ -171,7 +165,7 @@ void HomotopyClassPlanner::visualize()`
`171`	`165`	`visualization_->publishLocalPlanAndPoses(best_teb->teb());`
`172`	`166`
`173`	`167`	`if (best_teb->teb().sizePoses() > 0) //TODO maybe store current pose (start) within plan method as class field.`
`174`		`- visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *robot_model_);`
	`168`	`+ visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *cfg_->robot_model);`
`175`	`169`
`176`	`170`	`// feedback message`
`177`	`171`	`if (cfg_->trajectory.publish_feedback)`
`@@ -372,7 +366,7 @@ TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const PoseSE2& start`
`372`	`366`	`{`
`373`	`367`	`if(tebs_.size() >= cfg_->hcp.max_number_classes)`
`374`	`368`	`return TebOptimalPlannerPtr();`
`375`		`- TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, robot_model_, visualization_));`
	`369`	`+ TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, visualization_));`
`376`	`370`
`377`	`371`	`candidate->teb().initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);`
`378`	`372`
`@@ -427,7 +421,7 @@ TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const std::vector<ge`
`427`	`421`	`{`
`428`	`422`	`if(tebs_.size() >= cfg_->hcp.max_number_classes)`
`429`	`423`	`return TebOptimalPlannerPtr();`
`430`		`- TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, robot_model_, visualization_));`
	`424`	`+ TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, visualization_));`
`431`	`425`
`432`	`426`	`candidate->teb().initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,`
`433`	`427`	`cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);`