replace backward motion by inplace rotation. Warning: oscilliations c…

…an occure

include/teb_local_planner/homotopy_class_planner.h

@@ -406,6 +406,8 @@ class HomotopyClassPlanner : public PlannerInterface
    */
   virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
 
+  virtual bool getMeanVelocities(double up_to_dist, double& mean_vel_x, double& mean_vel_y, double& mean_vel_theta, bool& incl_backward_motion) const;
+
   /**
    * @brief Check if two h-signatures are similar (w.r.t. a certain threshold)
    * @param h1 first h-signature

include/teb_local_planner/optimal_planner.h

@@ -523,6 +523,8 @@ class TebOptimalPlanner : public PlannerInterface
    * @return \c true, if the planner suggests a shorter horizon, \c false otherwise.
    */
   virtual bool isHorizonReductionAppropriate(const std::vector<geometry_msgs::PoseStamped>& initial_plan) const;
+
+  virtual bool getMeanVelocities(double up_to_dist, double& mean_vel_x, double& mean_vel_y, double& mean_vel_theta, bool& incl_backward_motion) const;
 
   //@}
 

include/teb_local_planner/planner_interface.h

@@ -198,6 +198,16 @@ class PlannerInterface
   virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, bool alternative_time_cost=false)
   {
   }      
+
+  virtual bool getMeanVelocities(double up_to_dist, double& mean_vel_x, double& mean_vel_y, double& mean_vel_theta, bool& incl_backward_motion) const
+  {
+    ROS_WARN("Planner method 'virtual void getMeanVelocities(...) const' not implemented");
+    mean_vel_x = 0;
+    mean_vel_y = 0;
+    mean_vel_theta = 0;
+    incl_backward_motion = false;
+    return false;
+  }
 
 };
 

include/teb_local_planner/teb_config.h

@@ -97,6 +97,7 @@ class TebConfig
     double min_turning_radius; //!< Minimum turning radius of a carlike robot (diff-drive robot: zero); 
     double wheelbase; //!< The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!
     bool cmd_angle_instead_rotvel; //!< Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')
+    bool overwrite_backward_by_inplace_motion;
     bool is_footprint_dynamic; //<! If true, updated the footprint before checking trajectory feasibility
   } robot; //!< Robot related parameters
 
@@ -249,6 +250,7 @@ class TebConfig
     robot.min_turning_radius = 0;
     robot.wheelbase = 1.0;
     robot.cmd_angle_instead_rotvel = false;
+    robot.overwrite_backward_by_inplace_motion = true;
     robot.is_footprint_dynamic = false;
 
     // GoalTolerance

include/teb_local_planner/teb_local_planner_ros.h

@@ -342,6 +342,8 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner
    * @param min_obst_dist desired distance to obstacles
    */
   void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist);
+
+  bool rotateInPlaceIfBackward(geometry_msgs::Twist& cmd_vel);
 
 
   void configureBackupModes(std::vector<geometry_msgs::PoseStamped>& transformed_plan,  int& goal_idx);
@@ -390,6 +392,7 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner
   ros::Time time_last_oscillation_; //!< Store at which time stamp the last oscillation was detected
   RotType last_preferred_rotdir_; //!< Store recent preferred turning direction
   geometry_msgs::Twist last_cmd_; //!< Store the last control command generated in computeVelocityCommands()
+  bool inplace_rotation_active_; //!< Store, if the planner currently overwrites the optimizer by an inplace rotation
 
   std::vector<geometry_msgs::Point> footprint_spec_; //!< Store the footprint of the robot 
   double robot_inscribed_radius_; //!< The radius of the inscribed circle of the robot (collision possible)

src/homotopy_class_planner.cpp

@@ -687,5 +687,18 @@ void HomotopyClassPlanner::computeCurrentCost(std::vector<double>& cost, double
   }
 }
 
+bool HomotopyClassPlanner::getMeanVelocities(double up_to_dist, double& mean_vel_x, double& mean_vel_y, double& mean_vel_theta, bool& incl_backward_motion) const
+{
+  TebOptimalPlannerPtr best = bestTeb();
+  if (!best)
+  {
+    mean_vel_x = mean_vel_y = mean_vel_theta = 0;
+    incl_backward_motion = false;
+    return false;
+  }
+
+  return best->getMeanVelocities(up_to_dist, mean_vel_x, mean_vel_y, mean_vel_theta, incl_backward_motion);
+}
+
 
 } // end namespace

src/optimal_planner.cpp

@@ -1314,4 +1314,46 @@ bool TebOptimalPlanner::isHorizonReductionAppropriate(const std::vector<geometry
   return false;
 }
 
+bool TebOptimalPlanner::getMeanVelocities(double up_to_dist, double& mean_vel_x, double& mean_vel_y, double& mean_vel_theta, bool& incl_backward_motion) const
+{
+  double vel_x = 0;
+  double vel_y = 0;
+  double vel_theta = 0;
+
+  int num_values = 0;
+  mean_vel_x = 0;
+  mean_vel_y = 0;
+  mean_vel_theta = 0;
+  incl_backward_motion = false;
+
+  double dist = 0;
+
+  for (int i = 1; i < teb_.sizePoses(); ++i)
+  {
+    extractVelocity(teb_.Pose(i-1), teb_.Pose(i), teb_.TimeDiff(i-1), vel_x, vel_y, vel_theta);
+    mean_vel_x += vel_x;
+    mean_vel_y += vel_y;
+    mean_vel_theta += vel_theta;
+
+    dist += std::abs(teb_.TimeDiff(i-1) * vel_x);
+    if (dist >= up_to_dist)
+      break;
+
+    incl_backward_motion = incl_backward_motion || vel_x < 0;
+
+    ++num_values;
+  }
+
+  if (num_values > 0)
+  {
+    mean_vel_x /= (double)num_values;
+    mean_vel_y /= (double)num_values;
+    mean_vel_theta /= (double)num_values;
+  }
+  else
+    return false;
+
+  return true;
+}
+
 } // namespace teb_local_planner

src/teb_local_planner_ros.cpp

@@ -64,7 +64,7 @@ namespace teb_local_planner
 TebLocalPlannerROS::TebLocalPlannerROS() : costmap_ros_(NULL), tf_(NULL), costmap_model_(NULL),
                                            costmap_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"),
                                            dynamic_recfg_(NULL), custom_via_points_active_(false), goal_reached_(false), no_infeasible_plans_(0),
-                                           last_preferred_rotdir_(RotType::none), initialized_(false)
+                                           last_preferred_rotdir_(RotType::none), inplace_rotation_active_(false), initialized_(false)
 {
 }
 
@@ -369,6 +369,13 @@ bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
     last_cmd_ = cmd_vel;
     return false;
   }
+
+  if (cfg_.robot.overwrite_backward_by_inplace_motion)
+  {
+    rotateInPlaceIfBackward(cmd_vel);
+    //if (rotateInPlaceIfBackward(cmd_vel))
+      //ROS_INFO_STREAM("Backwards motion overwritten by inplace motion: ");
+  }
 
   // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).
   saturateVelocity(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, cfg_.robot.max_vel_x, cfg_.robot.max_vel_y,
@@ -391,7 +398,7 @@ bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
       return false;
     }
   }
-  
+
   // a feasible solution should be found, reset counter
   no_infeasible_plans_ = 0;
 
@@ -879,6 +886,67 @@ void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, double
                   "than the inscribed radius of the robot's footprint in the costmap parameters (%f, including 'footprint_padding'). "
                   "Infeasible optimziation results might occur frequently!", opt_inscribed_radius, min_obst_dist, costmap_inscribed_radius);
 }
+
+bool TebLocalPlannerROS::rotateInPlaceIfBackward(geometry_msgs::Twist& cmd_vel)
+{
+  double max_rot_vel = cfg_.robot.max_vel_theta;
+  double max_rot_acc = cfg_.robot.acc_lim_theta;
+
+  //double theta_diff = g2o::normalize_theta(ref_theta - current_pose.theta());
+
+  // check which side to rotate
+  // we do not want to screw up our optimized trajectory
+  // we could just check cmd_vel.linear.x and cmd_vel.angular.z but it turned out
+  // that checking directions along a small subset of the trajectory is more robust
+  double mean_vel_x = 0;
+  double mean_vel_y = 0;
+  double mean_vel_theta = 0;
+  bool incl_backward_motion = false;
+  if (!planner_->getMeanVelocities(0.25, mean_vel_x, mean_vel_y, mean_vel_theta, incl_backward_motion)) // TODO(roesmann): number of poses to check as parameter?
+  {
+    ROS_WARN("Cannot retrieve mean velocities");
+    return false;
+  }
+
+  if (!incl_backward_motion)
+  {
+    inplace_rotation_active_ = false;
+    return false;
+  }
+
+  double vel = max_rot_vel;
+  // choose direction of rotation only in case the inplace rotation is triggered now in order to avoid oscillations
+  if (inplace_rotation_active_)
+  {
+    if (last_cmd_.angular.z < 0) vel *= -1;
+    // ROS_INFO("active inplace rotation");
+  }
+  else
+  {
+    if (mean_vel_theta < 0) vel *= -1;
+    // ROS_INFO("new inplace rotation");
+  }
+
+
+
+  // chose velocity such we can stop at the reference given a maximum acceleration
+  //double vel = std::sqrt(2 * max_rot_acc * std::abs(theta_diff));
+  //vel = std::min(vel, max_rot_vel);
+  //vel *= -(double)g2o::sign(theta_diff);
+
+
+
+
+  cmd_vel.linear.x = 0.0;
+  cmd_vel.linear.y = 0.0;
+  cmd_vel.angular.z = vel;
+
+  // simulate rotation for collision checking (similar as in rotate_recovery.cpp of the navigation stack)
+  //double footprint_cost = c
+
+  inplace_rotation_active_ = true;
+  return true;
+}