- Aturada de gir quan max_vel = 0.

- optimitzada funcio de calcul maxim vel
- manicura de codi (usar _ enlloc de this-> ...)
- Testejat amb simulacio teo_sim

cfg/SafeCmd.cfg

@@ -38,11 +38,11 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 
 #       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
-gen.add("unsafe", bool_t,  0, "Not use the safe module", False)
-gen.add("min_dist", double_t, 0, "distance in meters where to stop", 0.4, 0, 2.0)
-gen.add("collision_time", double_t, 0, "time in seconds to compute collision", 1.0, 0.1, 5.0)
-gen.add("limit_vel_front", double_t, 0, "max speed in meters per second forward", 1.0, 0.0, 2.0)
-gen.add("limit_vel_rear", double_t, 0, "max speed in meters per second backward", 1.0, 0.0, 2.0)
+gen.add("unsafe",          bool_t,   0, "Not use the safe module",                 False)
+gen.add("min_dist",        double_t, 0, "distance in meters where to stop",        0.4, 0.0, 2.0)
+gen.add("collision_time",  double_t, 0, "time in seconds to compute collision",    1.0, 0.1, 5.0)
+gen.add("limit_vel_front", double_t, 0, "max speed in meters per second forward",  1.0, 0.0, 2.0)
+gen.add("limit_vel_rear",  double_t, 0, "max speed in meters per second backward", 1.0, 0.0, 2.0)
 #gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
 
 exit(gen.generate(PACKAGE, "SafeCmdAlgorithm", "SafeCmd"))

include/safe_cmd_alg_node.h

@@ -61,8 +61,8 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
     void front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
     CMutex front_laser_mutex_;
 
-    bool front_laser_received;
-    bool rear_laser_received;
+    bool front_laser_received_;
+    bool rear_laser_received_;
 
     // [service attributes]
 

launch/test_bag.launch

-<!-- This is a test for iri_robot_pose_ekf using a bag recorded with TEO robot-->
-<launch>
-
-  <arg name="bag" default="default.bag"/>
-  <param name="/use_sim_time" value="True"/>
-
-  <node pkg="rosbag" type="play" name="rosbag"  output="screen" args = "$(find bags)/$(arg bag) --pause --clock">
-  </node>
-
-
-  <node pkg="iri_safe_cmd"
-        type="iri_safe_cmd"
-        name="iri_safe_cmd"
-        output="screen"
-        >
-        <remap from="~cmd_vel"     to="/teo/segway/cmd_vel"/>
-        <remap from="~rear_laser"  to="/teo/sensors/rear_laser"/>
-        <remap from="~front_laser" to="/teo/sensors/front_laser"/>
-  </node>
-
-  
-</launch>
-

src/safe_cmd_alg_node.cpp

@@ -8,8 +8,8 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
   max_vel_rear_(7),
   limit_vel_front_(7),
   limit_vel_rear_(7),
-  front_laser_received(false),
-  rear_laser_received(false)
+  front_laser_received_(false),
+  rear_laser_received_(false)
 {
   //init class attributes if necessary
   loop_rate_ = 20;//in [Hz]
@@ -18,8 +18,8 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
   cmd_vel_safe_publisher_ = public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 100);
   
   // [init subscribers]
-  cmd_vel_subscriber_ = public_node_handle_.subscribe("cmd_vel", 100, &SafeCmdAlgNode::cmd_vel_callback,this);
-  rear_laser_subscriber_ = public_node_handle_.subscribe("rear_laser", 100, &SafeCmdAlgNode::rear_laser_callback, this);
+  cmd_vel_subscriber_     = public_node_handle_.subscribe("cmd_vel", 100, &SafeCmdAlgNode::cmd_vel_callback,this);
+  rear_laser_subscriber_  = public_node_handle_.subscribe("rear_laser", 100, &SafeCmdAlgNode::rear_laser_callback, this);
   front_laser_subscriber_ = public_node_handle_.subscribe("front_laser", 100, &SafeCmdAlgNode::front_laser_callback, this);
   
   // [init services]
@@ -39,26 +39,31 @@ SafeCmdAlgNode::~SafeCmdAlgNode(void)
 void SafeCmdAlgNode::mainNodeThread(void)
 {
   // [fill msg structures]
-  //this->Twist_msg_.data = my_var;
+  //Twist_msg_.data = my_var;
   
   if(!alg_.config_.unsafe)
   {
-    if(!this->front_laser_received || !this->rear_laser_received)
-      ROS_ERROR("SafeCmdAlgNode::mainNodeThread: laser/s not received");
+    if(!front_laser_received_ || !rear_laser_received_)
+      ROS_FATAL("SafeCmdAlgNode::mainNodeThread: laser/s not received");
+    
+    front_laser_received_ = false;
+    rear_laser_received_  = false;
 
     if(Twist_msg_.linear.x > fabs(max_vel_front_))
     {
+      ROS_WARN_STREAM("heading to front obstacle, reducing velocity"<<fabs(max_vel_front_));
       Twist_msg_.linear.x = fabs(max_vel_front_);
-      ROS_WARN("heading to front obstacle, reducing velocity");
+      if(max_vel_front_==0)
+        Twist_msg_.angular.z = 0;
     }
 
     if(Twist_msg_.linear.x < -fabs(max_vel_rear_))
     {
-      Twist_msg_.linear.x = -fabs(max_vel_rear_); 
-      ROS_WARN("heading to rear obstacle, reducing velocity");
+      ROS_WARN_STREAM("heading to rear obstacle, reducing velocity"<<fabs(max_vel_rear_));
+      Twist_msg_.linear.x = -fabs(max_vel_rear_);
+      if(max_vel_rear_==0)
+        Twist_msg_.angular.z = 0;
     }
-    this->front_laser_received = false;
-    this->rear_laser_received  = false;
   }
   // [fill srv structure and make request to the server]
   
@@ -75,8 +80,8 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
   //ROS_INFO("SafeCmdAlgNode::cmd_vel_callback: New Message Received"); 
 
   //use appropiate mutex to shared variables if necessary 
-  //this->alg_.lock(); 
-  //this->cmd_vel_mutex_.enter();
+  //alg_.lock(); 
+  //cmd_vel_mutex_.enter();
 
   if(!alg_.config_.unsafe)
   {
@@ -96,40 +101,40 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
     Twist_msg_ = *msg;
 
   //unlock previously blocked shared variables 
-  //this->alg_.unlock(); 
-  //this->cmd_vel_mutex_.exit(); 
+  //alg_.unlock(); 
+  //cmd_vel_mutex_.exit(); 
 }
 void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
 { 
   //ROS_INFO("SafeCmdAlgNode::rear_laser_callback: New Message Received"); 
 
   //use appropiate mutex to shared variables if necessary 
-  //this->alg_.lock(); 
-  //this->rear_laser_mutex_.enter(); 
+  //alg_.lock(); 
+  //rear_laser_mutex_.enter(); 
 
   max_vel_rear_ = std::min(compute_max_velocity_(msg),limit_vel_rear_);
+  rear_laser_received_ = true;
   //ROS_INFO("Max vel r: %f",max_vel_rear_);
-  this->rear_laser_received = true;
 
   //unlock previously blocked shared variables 
-  //this->alg_.unlock(); 
-  //this->rear_laser_mutex_.exit(); 
+  //alg_.unlock(); 
+  //rear_laser_mutex_.exit(); 
 }
 void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
 { 
   //ROS_INFO("SafeCmdAlgNode::front_laser_callback: New Message Received"); 
 
   //use appropiate mutex to shared variables if necessary 
-  //this->alg_.lock(); 
-  //this->front_laser_mutex_.enter(); 
+  //alg_.lock(); 
+  //front_laser_mutex_.enter(); 
 
   max_vel_front_ = std::min(compute_max_velocity_(msg),limit_vel_front_);
+  front_laser_received_ = true;
   //ROS_INFO("Max vel f: %f",max_vel_front_);
-  this->front_laser_received = true;
 
   //unlock previously blocked shared variables 
-  //this->alg_.unlock(); 
-  //this->front_laser_mutex_.exit(); 
+  //alg_.unlock(); 
+  //front_laser_mutex_.exit(); 
 }
 
 /*  [service callbacks] */
@@ -141,10 +146,10 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
 void SafeCmdAlgNode::node_config_update(Config &config, uint32_t level)
 {
   alg_.lock();
-  this->collision_time_ = config.collision_time;
-  this->min_dist_ = config.min_dist;
-  this->limit_vel_front_ = config.limit_vel_front;
-  this->limit_vel_rear_ = config.limit_vel_rear;
+  collision_time_  = config.collision_time;
+  min_dist_        = config.min_dist;
+  limit_vel_front_ = config.limit_vel_front;
+  limit_vel_rear_  = config.limit_vel_rear;
   alg_.unlock();
 }
 
@@ -160,13 +165,10 @@ int main(int argc,char *argv[])
 
 float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan) const
 {
-  float max_velocity;
+  float min_range    = *std::min_element( scan->ranges.begin(), scan->ranges.end() );
+  float max_velocity = 0;
 
-  float min_range = *std::min_element( scan->ranges.begin(), scan->ranges.end() );
-
-  if (min_range < min_dist_)
-    max_velocity = 0;
-  else
+  if (min_range >= min_dist_)
     max_velocity = min_range / collision_time_;
 
   return max_velocity;