Added a bypass parameter to be able to disable its operation when necessary.

cfg/SafeCmd.cfg

@@ -40,6 +40,7 @@ ackermann = gen.add_group("ackermann")
 
 #       Name       Type       Reconfiguration level    Description       Default   Min   Max
 gen.add("rate",                      double_t, 0, "Main loop rate (Hz)",  10.0,   0.1, 1000.0)
+gen.add("bypass",                    bool_t,   0, "Whether or not to bypassa the safety fwature", False)
 gen.add("safety_distance",           double_t, 0, "Safety distance to possible obstacles",0.5, 0.0, 10.0)
 gen.add("traj_sim_distance",         double_t, 0, "Distance for the trajectory computation",    1.0, 0.1, 5.0)
 gen.add("traj_sim_time",             double_t, 0, "Time for the trajectory computation",    1.0, 0.1, 5.0)

src/safe_cmd_alg_node.cpp

@@ -171,20 +171,25 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
   this->alg_.lock(); 
   //cmd_vel_mutex_.enter();
 
-  this->Twist_msg_=*msg;
-  // compute the robot trajectory depending on the architecture
-  if(this->config.architecture==0)// ackermann using steering angle
-    this->alg_.compute_trajectory_ackermann_steer_angle(msg->linear.x,msg->angular.z,this->traj);
-  else if(this->config.architecture==1)// ackermann using angular velocity
-    this->alg_.compute_trajectory_ackermann_angular_speed(msg->linear.x,msg->angular.z,this->traj);
-  else if(this->config.architecture==2)// differential
-    this->alg_.compute_trajectory_differential(msg->linear.x,msg->angular.z,this->traj);
+  if(this->config.bypass)
+    cmd_vel_safe_publisher_.publish(msg);
   else
   {
-    ROS_ERROR("SafeCmdAlgNode: unknown robot architecture");
-    return;
+    this->Twist_msg_=*msg;
+    // compute the robot trajectory depending on the architecture
+    if(this->config.architecture==0)// ackermann using steering angle
+      this->alg_.compute_trajectory_ackermann_steer_angle(msg->linear.x,msg->angular.z,this->traj);
+    else if(this->config.architecture==1)// ackermann using angular velocity
+      this->alg_.compute_trajectory_ackermann_angular_speed(msg->linear.x,msg->angular.z,this->traj);
+    else if(this->config.architecture==2)// differential
+      this->alg_.compute_trajectory_differential(msg->linear.x,msg->angular.z,this->traj);
+    else
+    {
+      ROS_ERROR("SafeCmdAlgNode: unknown robot architecture");
+      return;
+    }
+    this->new_cmd_vel=true;
   }
-  this->new_cmd_vel=true;
 
   //cmd_vel_mutex_.exit(); 
   this->alg_.unlock();