Add bool param to invert cmd_vel steering value. Add bool param to...

Add bool param to invert cmd_vel steering value. Add bool param to enable/disable inplace steering change before each maneuver

cfg/AckermannLocalPlanner.cfg

@@ -63,6 +63,8 @@ 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("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

@@ -148,5 +148,7 @@ class AckermannPlannerROS : public nav_core::BaseLocalPlanner
     bool stucked;
     bool first;
     bool use_steer_angle_cmd;
+    bool stopped_steering;
+    bool invert_steer_angle_cmd;
 };
 #endif

src/ackermann_planner_ros.cpp

@@ -100,6 +100,8 @@ void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::Acke
   planner_util_.reconfigure_callback(limits, config.restore_defaults);
   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;
   odom_helper_.set_average_samples(config.odom_avg);
 
   patience_=config.planner_patience;
@@ -216,7 +218,10 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
   //pass along drive commands
   cmd_vel.linear.x = drive_cmds.pose.position.x;
   cmd_vel.linear.y = drive_cmds.pose.position.y;
-  cmd_vel.angular.z = tf::getYaw(drive_cmds.pose.orientation);
+  if(this->invert_steer_angle_cmd)
+    cmd_vel.angular.z = -tf::getYaw(drive_cmds.pose.orientation);
+  else
+    cmd_vel.angular.z = tf::getYaw(drive_cmds.pose.orientation);
 
   //if we cannot move... tell someone
   std::vector<geometry_msgs::PoseStamped> local_plan;
@@ -442,7 +447,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
       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)
+      if(this->use_steer_angle_cmd && this->stopped_steering)
       {
         if(fabs(ackermann.steering_angle-cmd_vel.angular.z)>0.05)
         {