Rename stucked with stuck. Add parameters to enable/disable the stuck check...

Rename stucked with stuck. Add parameters to enable/disable the stuck check and change the vel threshold and max occurrences.

cfg/AckermannLocalPlanner.cfg

@@ -63,8 +63,13 @@ gen.add("trans_vel_samples",       int_t,     0, "The number of samples to use w
 gen.add("steer_angle_samples",     int_t,     0, "The number of samples to use when exploring the steer angle space",                                                          10, 1)
 gen.add("angular_vel_samples",     int_t,     0, "The number of samples to use when exploring the angular velocity space",                                                     10, 1)
 gen.add("use_steer_angle_cmd",     bool_t,    0, "Whether to use the steer angle or the angular velocity.",                                                                    False)
-gen.add("stopped_steering",        bool_t,    0, "Whether to allow steering while stopped or not.",                                                                            True)
 gen.add("invert_steer_angle_cmd",  bool_t,    0, "Whether to use the invert the steer angle command or not.",                                                                  False)
+gen.add("use_stopped_steering",    bool_t,    0, "Whether to allow steering while stopped or not.",                                                                            True)
+gen.add("stopped_steering_angle_threshold", double_t,  0, "Angle difference threshold (rad) to perform steering change while stopped.",                                              0.05, 0)
+
+gen.add("use_stuck_check",       bool_t,    0, "Whether to allow robot stuck check or not.",                                                                               True)
+gen.add("stuck_check_vel_threshold", double_t,  0, "Velocity threshold (m/s) to consider robot stuck.",                                              0.05, 0)
+gen.add("stuck_check_max_occurrences", int_t,  0, "Max number of stuck occurrences allowed until moving to next segment or replaning.",                                              10, 0)
 
 gen.add("prune_plan",              bool_t,    0, "Start following closest point of global plan, not first point (if different).",                                              False)
 gen.add("xy_goal_tolerance",       double_t,  0, "Within what maximum distance we consider the robot to be in goal",                                                           0.1)

include/ackermann_planner_ros.h

@@ -145,10 +145,15 @@ class AckermannPlannerROS : public nav_core::BaseLocalPlanner
     std::string odom_topic_;
     std::string ackermann_topic_;
 
-    bool stucked;
+    bool stuck;
     bool first;
     bool use_steer_angle_cmd;
-    bool stopped_steering;
     bool invert_steer_angle_cmd;
+    bool use_stopped_steering;
+    double stopped_steering_angle_threshold;
+    bool use_stuck_check;
+    double stuck_check_vel_threshold;
+    int stuck_check_max_occurrences;
+
 };
 #endif

src/ackermann_planner_ros.cpp

@@ -55,7 +55,7 @@ AckermannPlannerROS::AckermannPlannerROS() : initialized_(false),
   odom_helper_("odom","ackermann_feedback",10), setup_(false) 
 {
   patience_=1;
-  this->stucked=false;
+  this->stuck=false;
   this->first=true;
 }
 
@@ -101,7 +101,13 @@ void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::Acke
   this->last_cmds.resize(config.cmd_vel_avg);
   this->use_steer_angle_cmd=config.use_steer_angle_cmd;
   this->invert_steer_angle_cmd=config.invert_steer_angle_cmd;
-  this->stopped_steering=config.stopped_steering;
+  this->use_stopped_steering=config.use_stopped_steering;
+
+  this->stopped_steering_angle_threshold=config.stopped_steering_angle_threshold;
+  this->use_stuck_check = config.use_stuck_check;
+  this->stuck_check_vel_threshold=config.stuck_check_vel_threshold;
+  this->stuck_check_max_occurrences=config.stuck_check_max_occurrences;
+
   odom_helper_.set_average_samples(config.odom_avg);
 
   patience_=config.planner_patience;
@@ -143,7 +149,7 @@ void AckermannPlannerROS::initialize(std::string name,tf2_ros::Buffer* tf,costma
     dsrv_ = new dynamic_reconfigure::Server<iri_ackermann_local_planner::AckermannLocalPlannerConfig>(private_nh);
     dynamic_reconfigure::Server<iri_ackermann_local_planner::AckermannLocalPlannerConfig>::CallbackType cb = boost::bind(&AckermannPlannerROS::reconfigure_callback, this, _1, _2);
     dsrv_->setCallback(cb);
-    this->stucked=false;
+    this->stuck=false;
     this->first=true;
   }
   else
@@ -160,7 +166,7 @@ bool AckermannPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>&
   }
   //when we get a new plan, we also want to clear any latch we may have on goal tolerances
   // latchedStopRotateController_.resetLatching();
-  this->stucked=false;
+  this->stuck=false;
   this->first=true;
   for(unsigned int i=0;i<this->last_cmds.size();i++)
     this->last_cmds[i].linear.x=0.0;
@@ -304,7 +310,7 @@ bool AckermannPlannerROS::isGoalReached(void)
       ROS_INFO("AckermannPlannerROS: Goal reached");
       return true;
     }
-    else if(this->stucked)
+    else if(this->stuck)
       return true;
     else
       return false;
@@ -346,7 +352,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
   nav_msgs::Odometry odom;
   AckermannPlannerLimits limits;
   geometry_msgs::PoseStamped goal_pose;
-  static int stucked_count=0;
+  static int stuck_count=0;
   static int replan_count=0;
   static bool new_segment=false;
 
@@ -357,10 +363,10 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
     cmd_vel.linear.x = 0.0;
     cmd_vel.linear.y = 0.0;
     cmd_vel.angular.z = 0.0;
-    this->stucked=false;
+    this->stuck=false;
     new_segment=false;
     replan_count=0;
-    stucked_count=0;
+    stuck_count=0;
     return false;
   }
   std::vector<geometry_msgs::PoseStamped> transformed_plan;
@@ -370,10 +376,10 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
     cmd_vel.linear.x = 0.0;
     cmd_vel.linear.y = 0.0;
     cmd_vel.angular.z = 0.0;
-    this->stucked=false;
+    this->stuck=false;
     new_segment=false;
     replan_count=0;
-    stucked_count=0;
+    stuck_count=0;
     return false;
   }
 
@@ -384,10 +390,10 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
     cmd_vel.linear.x = 0.0;
     cmd_vel.linear.y = 0.0;
     cmd_vel.angular.z = 0.0;
-    this->stucked=false;
+    this->stuck=false;
     new_segment=false;
     replan_count=0;
-    stucked_count=0;
+    stuck_count=0;
     return false;
   }
   ROS_DEBUG_NAMED("ackermann_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());
@@ -397,7 +403,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
 
   odom_helper_.get_odom(odom);
   limits=planner_util_.get_current_limits();
-  if(this->stucked || base_local_planner::isGoalReached(*tf_,
+  if(this->stuck || base_local_planner::isGoalReached(*tf_,
                                                         transformed_plan,
                                                         *costmap_ros_->getCostmap(),
                                                         costmap_ros_->getGlobalFrameID(),
@@ -408,7 +414,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
   {
     if(planner_util_.set_next_path())
     {
-      this->stucked=false;
+      this->stuck=false;
       new_segment=true;
       bool isOk = ackermann_compute_velocity_commands(current_pose_, tmp_cmd_vel);
       cmd_vel=this->average_cmd_vel(tmp_cmd_vel);
@@ -431,30 +437,33 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
       cmd_vel.linear.x = 0.0;
       cmd_vel.linear.y = 0.0;
       cmd_vel.angular.z = 0.0;
-      this->stucked=false;
+      this->stuck=false;
       new_segment=false;
       replan_count=0;
-      stucked_count=0;
+      stuck_count=0;
     }
   }
   else
   {
     bool isOk = ackermann_compute_velocity_commands(current_pose_, tmp_cmd_vel);
     cmd_vel=this->average_cmd_vel(tmp_cmd_vel);
-    // check wether the robot get stucked or not
+    // check wether the robot get stuck or not
     if(new_segment || this->first)
     {
       ackermann_msgs::AckermannDrive ackermann;
       odom_helper_.get_ackermann_state(ackermann);
       // steer the car without moving if the steering angle is big
-      if(this->use_steer_angle_cmd && this->stopped_steering)
+      if(this->use_steer_angle_cmd && this->use_stopped_steering)
       {
         double ang_z = cmd_vel.angular.z;
         if(this->invert_steer_angle_cmd)
           ang_z = -ang_z;
-        if(fabs(ackermann.steering_angle-ang_z)>0.05)
+
+        double diff = fabs(ackermann.steering_angle-ang_z);
+        if(diff > this->stopped_steering_angle_threshold)
         {
-          //ROS_WARN("AckermannPlannerROS: steering without moving. Setting forward speed to 0 (%f,%f)",ackermann.steering_angle,cmd_vel.angular.z);
+          //ROS_INFO("AckermannPlannerROS: steering without moving")
+          //ROS_INFO("AckermannPlannerROS: stopped steering. Angle current - target = %f - %f = %f, vs threshold %f )",ackermann.steering_angle, ang_z, diff, this->stopped_steering_angle_threshold);
           cmd_vel.linear.x=0.0;
         }
         else
@@ -473,7 +482,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
           new_segment=false;
       }
     }
-    else if(fabs(cmd_vel.linear.x)<limits.trans_vel_deadzone)
+    else if(fabs(cmd_vel.linear.x)<this->stuck_check_vel_threshold)
     {
        //ROS_INFO_STREAM("linear.x " << cmd_vel.linear.x << " deadzone: " << limits.trans_vel_deadzone );
        // get the position of the goal
@@ -485,26 +494,26 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
        double dist=base_local_planner::getGoalPositionDistance(current_pose_,goal_pose.pose.position.x,goal_pose.pose.position.y);
        if(dist>limits.xy_goal_tolerance && dist<2*limits.xy_goal_tolerance)
        {
-         stucked_count++;
-         if(stucked_count>10)
+         stuck_count++;
+         if(stuck_count>this->stuck_check_max_occurrences)
          {
-           this->stucked=true;
-           ROS_INFO("AckermannPlannerROS: Robot may be stucked near segment end point. Jumping to the next path segment");
+           this->stuck=true;
+           ROS_INFO("AckermannPlannerROS: Robot may be stuck near segment end point. Jumping to the next path segment");
          }
        }
        else
        {
          replan_count++;
-         if(replan_count>10)
+         if(replan_count>this->stuck_check_max_occurrences)
          {
-           ROS_INFO("AckermannPlannerROS: Robot seems stucked. Replanning");
+           ROS_INFO("AckermannPlannerROS: Robot seems stuck. Replanning");
            cmd_vel.linear.x = 0.0;
            cmd_vel.linear.y = 0.0;
            cmd_vel.angular.z = 0.0;
-           this->stucked=false;
+           this->stuck=false;
            new_segment=false;
            replan_count=0;
-           stucked_count=0;
+           stuck_count=0;
            return false;
          }
        }
@@ -512,7 +521,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
     }
     else
     {
-      stucked_count=0;
+      stuck_count=0;
       replan_count=0;
     }
 
@@ -535,10 +544,10 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
       cmd_vel.linear.x = 0.0;
       cmd_vel.linear.y = 0.0;
       cmd_vel.angular.z = 0.0;
-      this->stucked=false;
+      this->stuck=false;
       new_segment=false;
       replan_count=0;
-      stucked_count=0;
+      stuck_count=0;
     }
     
     return isOk;