Added the rate parameter.

cfg/SafeCmd.cfg

@@ -39,7 +39,8 @@ 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("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);