Add possibility to weight viapoint orientations

cfg/TebLocalPlannerReconfigure.cfg

@@ -269,6 +269,10 @@ grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0,
 grp_optimization.add("weight_viapoint", double_t, 0, 
   "Optimization weight for minimizing the distance to via-points", 
   1, 0, 1000) 
+
+grp_optimization.add("weight_viapoint_orientation", double_t, 0, 
+  "Optimization weight for minimizing the orientation error w.r.t via-points", 
+  1, 0, 1000) 
 
 grp_optimization.add("weight_adapt_factor", double_t, 0, 
   "Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.", 

include/teb_local_planner/g2o_types/edge_via_point.h

@@ -63,7 +63,7 @@ namespace teb_local_planner
  * @see TebOptimalPlanner::AddEdgesViaPoints
  * @remarks Do not forget to call setTebConfig() and setViaPoint()
  */     
-class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose>
+class EdgeViaPoint : public BaseTebUnaryEdge<2, const PoseSE2*, VertexPose>
 {
 public:
 
@@ -83,7 +83,8 @@ class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPo
     ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()");
     const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
 
-    _error[0] = (bandpt->position() - *_measurement).norm();
+    _error[0] = (bandpt->position() - _measurement->position()).norm(); // as in EdgeViaPoint
+    _error[1] = g2o::normalize_theta(bandpt->theta() - _measurement->theta());
 
     ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]);
   }
@@ -92,7 +93,7 @@ class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPo
    * @brief Set pointer to associated via point for the underlying cost function 
    * @param via_point 2D position vector containing the position of the via point
    */ 
-  void setViaPoint(const Eigen::Vector2d* via_point)
+  void setViaPoint(const PoseSE2* via_point)
   {
     _measurement = via_point;
   }
@@ -102,7 +103,7 @@ class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPo
    * @param cfg TebConfig class
    * @param via_point 2D position vector containing the position of the via point
    */ 
-  void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point)
+  void setParameters(const TebConfig& cfg, const PoseSE2* via_point)
   {
     cfg_ = &cfg;
     _measurement = via_point;

include/teb_local_planner/optimal_planner.h

@@ -89,7 +89,7 @@ typedef g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> TEBLinearSolver
 //typedef g2o::LinearSolverCholmod<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
 
 //! Typedef for a container storing via-points
-typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ViaPointContainer;
+typedef std::vector< PoseSE2, Eigen::aligned_allocator<PoseSE2> > ViaPointContainer;
 
 
 /**

include/teb_local_planner/teb_config.h

@@ -154,6 +154,7 @@ class TebConfig
     double weight_dynamic_obstacle; //!< Optimization weight for satisfying a minimum separation from dynamic obstacles    
     double weight_dynamic_obstacle_inflation; //!< Optimization weight for the inflation penalty of dynamic obstacles (should be small)
     double weight_viapoint; //!< Optimization weight for minimizing the distance to via-points
+    double weight_viapoint_orientation; //!< Optimization weight for minimizing the orientation error v.r.t via-points
     double weight_prefer_rotdir; //!< Optimization weight for preferring a specific turning direction (-> currently only activated if an oscillation is detected, see 'oscillation_recovery'
 
     double weight_adapt_factor; //!< Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new = weight_old*factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.
@@ -296,6 +297,7 @@ class TebConfig
     optim.weight_dynamic_obstacle = 50;
     optim.weight_dynamic_obstacle_inflation = 0.1;
     optim.weight_viapoint = 1;
+    optim.weight_viapoint_orientation = 1;
     optim.weight_prefer_rotdir = 50;
 
     optim.weight_adapt_factor = 2.0;

include/teb_local_planner/visualization.h

@@ -147,7 +147,7 @@ class TebVisualization
    * @brief Publish via-points to the ros topic \e ../../teb_markers
    * @param via_points via-point container
    */
-  void publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns = "ViaPoints") const;
+  void publishViaPoints(const std::vector< PoseSE2, Eigen::aligned_allocator<PoseSE2> >& via_points, const std::string& ns = "ViaPoints") const;
 
   /**
    * @brief Publish a boost::adjacency_list (boost's graph datatype) via markers.

src/homotopy_class_planner.cpp

@@ -295,7 +295,7 @@ void HomotopyClassPlanner::renewAndAnalyzeOldTebs(bool delete_detours)
 
 void HomotopyClassPlanner::updateReferenceTrajectoryViaPoints(bool all_trajectories)
 {
-  if ( (!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || cfg_->optim.weight_viapoint <= 0)
+  if ( (!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || (cfg_->optim.weight_viapoint <= 0 && cfg_->optim.weight_viapoint_orientation))
     return;
 
   if(equivalence_classes_.size() < tebs_.size())

src/optimal_planner.cpp

@@ -664,7 +664,7 @@ void TebOptimalPlanner::AddEdgesDynamicObstacles(double weight_multiplier)
 
 void TebOptimalPlanner::AddEdgesViaPoints()
 {
-  if (cfg_->optim.weight_viapoint==0 || via_points_==NULL || via_points_->empty() )
+  if ((cfg_->optim.weight_viapoint==0 && cfg_->optim.weight_viapoint_orientation==0)|| via_points_==NULL || via_points_->empty() )
     return; // if weight equals zero skip adding edges!
 
   int start_pose_idx = 0;
@@ -675,8 +675,9 @@ void TebOptimalPlanner::AddEdgesViaPoints()
 
   for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it)
   {
+	PoseSE2 temp = *vp_it;
 
-    int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx);
+    int index = teb_.findClosestTrajectoryPose(temp.position(), NULL, start_pose_idx);
     if (cfg_->trajectory.via_points_ordered)
       start_pose_idx = index+2; // skip a point to have a DOF inbetween for further via-points
 
@@ -696,9 +697,12 @@ void TebOptimalPlanner::AddEdgesViaPoints()
         continue; // skip via points really close or behind the current robot pose
       }
     }
-    Eigen::Matrix<double,1,1> information;
-    information.fill(cfg_->optim.weight_viapoint);
 
+    Eigen::Matrix<double,2,2> information;
+    information(0,0) = cfg_->optim.weight_viapoint;
+    information(1,1) = cfg_->optim.weight_viapoint_orientation;
+    information(0,1) = information(1,0) = 0;
+
     EdgeViaPoint* edge_viapoint = new EdgeViaPoint;
     edge_viapoint->setVertex(0,teb_.PoseVertex(index));
     edge_viapoint->setInformation(information);

src/teb_config.cpp

@@ -119,6 +119,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
   nh.param("weight_dynamic_obstacle", optim.weight_dynamic_obstacle, optim.weight_dynamic_obstacle);    
   nh.param("weight_dynamic_obstacle_inflation", optim.weight_dynamic_obstacle_inflation, optim.weight_dynamic_obstacle_inflation);
   nh.param("weight_viapoint", optim.weight_viapoint, optim.weight_viapoint);
+  nh.param("weight_viapoint_orientation", optim.weight_viapoint_orientation, optim.weight_viapoint_orientation);
   nh.param("weight_prefer_rotdir", optim.weight_prefer_rotdir, optim.weight_prefer_rotdir);
   nh.param("weight_adapt_factor", optim.weight_adapt_factor, optim.weight_adapt_factor);
 
@@ -227,6 +228,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
   optim.weight_dynamic_obstacle = cfg.weight_dynamic_obstacle;
   optim.weight_dynamic_obstacle_inflation = cfg.weight_dynamic_obstacle_inflation;
   optim.weight_viapoint = cfg.weight_viapoint;
+  optim.weight_viapoint_orientation = cfg.weight_viapoint_orientation;
   optim.weight_adapt_factor = cfg.weight_adapt_factor;
 
   // Homotopy Class Planner

src/teb_local_planner_ros.cpp

@@ -587,8 +587,17 @@ void TebLocalPlannerROS::updateViaPointsContainer(const std::vector<geometry_msg
     if (distance_points2d( transformed_plan[prev_idx].pose.position, transformed_plan[i].pose.position ) < min_separation)
       continue;
 
+    tf::Quaternion q(
+    	transformed_plan[i].pose.orientation.x,
+		transformed_plan[i].pose.orientation.y,
+		transformed_plan[i].pose.orientation.z,
+		transformed_plan[i].pose.orientation.w);
+    tf::Matrix3x3 m(q);
+    double roll, pitch, yaw;
+    m.getRPY(roll, pitch, yaw);
+
     // add via-point
-    via_points_.push_back( Eigen::Vector2d( transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y ) );
+    via_points_.push_back( PoseSE2( transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y, yaw) );
     prev_idx = i;
   }
 
@@ -978,7 +987,16 @@ void TebLocalPlannerROS::customViaPointsCB(const nav_msgs::Path::ConstPtr& via_p
   via_points_.clear();
   for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses)
   {
-    via_points_.emplace_back(pose.pose.position.x, pose.pose.position.y);
+	tf::Quaternion q(
+		pose.pose.orientation.x,
+		pose.pose.orientation.y,
+		pose.pose.orientation.z,
+		pose.pose.orientation.w);
+	tf::Matrix3x3 m(q);
+	double roll, pitch, yaw;
+	m.getRPY(roll, pitch, yaw);
+
+    via_points_.emplace_back(pose.pose.position.x, pose.pose.position.y, yaw);
   }
   custom_via_points_active_ = !via_points_.empty();
 }

src/test_optim_node.cpp

@@ -282,7 +282,7 @@ void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg)
 {
   // we assume for simplicity that the fixed frame is already the map/planning frame
   // consider clicked points as via-points
-  via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) );
+  via_points.emplace_back(point_msg->point.x, point_msg->point.y, 0);
   ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added.");
   if (config.optim.weight_viapoint<=0)
     ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0");
@@ -294,7 +294,7 @@ void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg)
   via_points.clear();
   for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses)
   {
-    via_points.emplace_back(pose.pose.position.x, pose.pose.position.y);
+    via_points.emplace_back(pose.pose.position.x, pose.pose.position.y, 0);
   }
 }
 

src/visualization.cpp

@@ -294,7 +294,7 @@ void TebVisualization::publishObstacles(const ObstContainer& obstacles) const
 }
 
 
-void TebVisualization::publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns) const
+void TebVisualization::publishViaPoints(const std::vector< PoseSE2, Eigen::aligned_allocator<PoseSE2> >& via_points, const std::string& ns) const
 {
   if ( via_points.empty() || printErrorWhenNotInitialized() )
     return;