Merge branch 'adc' into 'master'

Adc

See merge request !2

cfg/SafeCmd.cfg

@@ -39,7 +39,9 @@ gen = ParameterGenerator()
 ackermann = gen.add_group("ackermann")
 
 #       Name       Type       Reconfiguration level    Description       Default   Min   Max
-gen.add("safety_distance",           double_t, 0, "Safety distance to possible obstacles",0.5, 0.0, 2.0)
+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)
 gen.add("traj_sim_step",             double_t, 0, "Distance/Time increment for the trajectory computation",    0.1, 0.01, 0.5)

param/sample.yaml

+rate: 10
 footprint: [[0.7,0.15],[0.7,-0.15],[-0.35,-0.15],[-0.35,0.15]]
 safety_distance: 0.2
 traj_sim_distance: 2.0

src/safe_cmd_alg_node.cpp

@@ -12,7 +12,12 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
   tf2_listener(tf2_buffer)
 {
   //init class attributes if necessary
-  this->setRate(30);//in [Hz]
+  if(!this->private_node_handle_.getParam("rate", this->config.rate))
+  {
+    ROS_WARN("SafeCmdAlgNode::SafeCmdAlgNode: param 'rate' not found");
+  }
+  else
+    this->setRate(this->config.rate);
 
   // [init publishers]
   this->collision_points_publisher_ = this->private_node_handle_.advertise<sensor_msgs::PointCloud2>("collision_points", 1);
@@ -166,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();