fix segfault at robot-model update (rst-tu-dortmund#255 revisited) (rst-tu-dortmund#393)

Co-authored-by: Dima Dorezyuk <dorezyuk@magazino.eu>

Author: Jorge Santos Simón

include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h

@@ -80,7 +80,7 @@ class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPo
   }
 
   /**
-   * @brief Construct edge and specify the time for its associated pose (neccessary for computeError).
+   * @brief Construct edge and specify the time for its associated pose (necessary for computeError).
    * @param t_ Estimated time until current pose is reached
    */      
   EdgeDynamicObstacle(double t) : t_(t)
@@ -92,10 +92,10 @@ class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPo
    */   
   void computeError()
   {
-    ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeDynamicObstacle()");
+    ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setObstacle() on EdgeDynamicObstacle()");
     const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
 
-    double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_);
+    double dist = cfg_->robot_model->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_);
 
     _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
     _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.dynamic_obstacle_inflation_dist, 0.0);
@@ -113,40 +113,26 @@ class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPo
     _measurement = obstacle;
   }
 
-  /**
-   * @brief Set pointer to the robot model
-   * @param robot_model Robot model required for distance calculation
-   */
-  void setRobotModel(const BaseRobotFootprintModel* robot_model)
-  {
-    robot_model_ = robot_model;
-  }
-
   /**
    * @brief Set all parameters at once
    * @param cfg TebConfig class
-   * @param robot_model Robot model required for distance calculation
    * @param obstacle 2D position vector containing the position of the obstacle
    */
-  void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+  void setParameters(const TebConfig& cfg, const Obstacle* obstacle)
   {
     cfg_ = &cfg;
-    robot_model_ = robot_model;
     _measurement = obstacle;
   }
 
 protected:
 
-  const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
   double t_; //!< Estimated time until current pose is reached
 
 public: 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
 };
-    
- 
-    
+
 
 } // end namespace
 

include/teb_local_planner/g2o_types/edge_obstacle.h

@@ -84,10 +84,10 @@ class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose>
    */    
   void computeError()
   {
-    ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()");
+    ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setObstacle() on EdgeObstacle()");
     const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
 
-    double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
+    double dist = cfg_->robot_model->calculateDistance(bandpt->pose(), _measurement);
 
     // Original obstacle cost.
     _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
@@ -159,35 +159,18 @@ class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose>
     _measurement = obstacle;
   }
 
-  /**
-   * @brief Set pointer to the robot model 
-   * @param robot_model Robot model required for distance calculation
-   */ 
-  void setRobotModel(const BaseRobotFootprintModel* robot_model)
-  {
-    robot_model_ = robot_model;
-  }
-    
   /**
    * @brief Set all parameters at once
    * @param cfg TebConfig class
-   * @param robot_model Robot model required for distance calculation
    * @param obstacle 2D position vector containing the position of the obstacle
    */ 
-  void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+  void setParameters(const TebConfig& cfg, const Obstacle* obstacle)
   {
     cfg_ = &cfg;
-    robot_model_ = robot_model;
     _measurement = obstacle;
   }
 
-protected:
-
-  const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
-  
-public: 	
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
 };
 
 
@@ -223,10 +206,10 @@ class EdgeInflatedObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexP
    */    
   void computeError()
   {
-    ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeInflatedObstacle()");
+    ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setObstacle() on EdgeInflatedObstacle()");
     const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
 
-    double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
+    double dist = cfg_->robot_model->calculateDistance(bandpt->pose(), _measurement);
 
     // Original "straight line" obstacle cost. The max possible value
     // before weighting is min_obstacle_dist
@@ -257,36 +240,19 @@ class EdgeInflatedObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexP
   {
     _measurement = obstacle;
   }
-    
-  /**
-   * @brief Set pointer to the robot model 
-   * @param robot_model Robot model required for distance calculation
-   */ 
-  void setRobotModel(const BaseRobotFootprintModel* robot_model)
-  {
-    robot_model_ = robot_model;
-  }
-    
+
   /**
    * @brief Set all parameters at once
    * @param cfg TebConfig class
-   * @param robot_model Robot model required for distance calculation
    * @param obstacle 2D position vector containing the position of the obstacle
    */ 
-  void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+  void setParameters(const TebConfig& cfg, const Obstacle* obstacle)
   {
     cfg_ = &cfg;
-    robot_model_ = robot_model;
     _measurement = obstacle;
   }
 
-protected:
-
-  const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
-  
-public:         
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
 };
 
 

include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h

@@ -70,8 +70,7 @@ class EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*>
   /**
    * @brief Construct edge.
    */
-  EdgeVelocityObstacleRatio() :
-    robot_model_(nullptr)
+  EdgeVelocityObstacleRatio()
   {
     // The three vertices are two poses and one time difference
     this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
@@ -82,7 +81,7 @@ class EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*>
    */
   void computeError()
   {
-    ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeVelocityObstacleRatio()");
+    ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setObstacle() on EdgeVelocityObstacleRatio()");
     const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
     const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
     const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
@@ -102,7 +101,7 @@ class EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*>
 
     const double omega = angle_diff / deltaT->estimate();
 
-    double dist_to_obstacle = robot_model_->calculateDistance(conf1->pose(), _measurement);
+    double dist_to_obstacle = cfg_->robot_model->calculateDistance(conf1->pose(), _measurement);
 
     double ratio;
     if (dist_to_obstacle < cfg_->obstacles.obstacle_proximity_lower_bound)
@@ -131,36 +130,19 @@ class EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*>
     _measurement = obstacle;
   }
 
-  /**
-   * @brief Set pointer to the robot model
-   * @param robot_model Robot model required for distance calculation
-   */
-  void setRobotModel(const BaseRobotFootprintModel* robot_model)
-  {
-    robot_model_ = robot_model;
-  }
-
   /**
    * @brief Set all parameters at once
    * @param cfg TebConfig class
-   * @param robot_model Robot model required for distance calculation
    * @param obstacle 2D position vector containing the position of the obstacle
    */
-  void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+  void setParameters(const TebConfig& cfg, const Obstacle* obstacle)
   {
     cfg_ = &cfg;
-    robot_model_ = robot_model;
     _measurement = obstacle;
   }
 
-protected:
-
-  const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
-
-public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
 };
 
-
 } // end namespace

include/teb_local_planner/homotopy_class_planner.h

@@ -120,11 +120,10 @@ class HomotopyClassPlanner : public PlannerInterface
    * @brief Construct and initialize the HomotopyClassPlanner
    * @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)
    */
-  HomotopyClassPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
+  HomotopyClassPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL,
                        TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
 
   /**
@@ -136,16 +135,13 @@ class HomotopyClassPlanner : public PlannerInterface
    * @brief Initialize the HomotopyClassPlanner
    * @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(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
+  void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL,
                   TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
 
 
-  void updateRobotModel(RobotFootprintModelPtr robot_model );
-
   /** @name Plan a trajectory */
   //@{
 
@@ -553,7 +549,6 @@ class HomotopyClassPlanner : public PlannerInterface
   TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...)
   TebOptimalPlannerPtr best_teb_; //!< Store the current best teb.
   EquivalenceClassPtr best_teb_eq_class_; //!< Store the equivalence class of the current best teb
-  RobotFootprintModelPtr robot_model_; //!< Robot model shared instance
 
   const std::vector<geometry_msgs::PoseStamped>* initial_plan_; //!< Store the initial plan if available for a better trajectory initialization
   EquivalenceClassPtr initial_plan_eq_class_; //!< Store the equivalence class of the initial plan

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::Twist* start_velocity, bool free_goal_vel)
 {
-  TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(*cfg_, obstacles_, robot_model_));
+  TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(*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,

include/teb_local_planner/optimal_planner.h

@@ -110,11 +110,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(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
+  TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL,
                     TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
 
   /**
@@ -126,18 +125,12 @@ class TebOptimalPlanner : public PlannerInterface
     * @brief Initializes the 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)
     */
-  void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
+  void initialize(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  */
   //@{
 
@@ -690,7 +683,6 @@ class TebOptimalPlanner : public PlannerInterface
   // internal objects (memory management owned)
   TebVisualizationPtr visualization_; //!< Instance of the visualization class
   TimedElasticBand teb_; //!< Actual trajectory object
-  RobotFootprintModelPtr robot_model_; //!< Robot model
   boost::shared_ptr<g2o::SparseOptimizer> optimizer_; //!< g2o optimizer for trajectory optimization
   std::pair<bool, geometry_msgs::Twist> vel_start_; //!< Store the initial velocity at the start pose
   std::pair<bool, geometry_msgs::Twist> vel_goal_; //!< Store the final velocity at the goal pose

include/teb_local_planner/teb_config.h

@@ -45,6 +45,7 @@
 #include <Eigen/StdVector>
 
 #include <teb_local_planner/TebLocalPlannerReconfigureConfig.h>
+#include <teb_local_planner/robot_footprint_model.h>
 
 
 // Definitions
@@ -65,6 +66,8 @@ class TebConfig
   std::string odom_topic; //!< Topic name of the odometry message, provided by the robot driver or simulator
   std::string map_frame; //!< Global planning frame
 
+  RobotFootprintModelPtr robot_model; //!< model of the robot's footprint
+
   //! Trajectory related parameters
   struct Trajectory
   {
@@ -244,6 +247,7 @@ class TebConfig
 
     odom_topic = "odom";
     map_frame = "odom";
+    robot_model = boost::make_shared<PointRobotFootprint>();
 
     // Trajectory