Add missing watchdogs reset on constructor. Fix twist variable msg published...

Add missing watchdogs reset on constructor. Fix twist variable msg published on watchdog activation. Do not publish twist message if no laser data has been received

src/safe_cmd_alg_node.cpp

@@ -22,6 +22,9 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
   this->cmd_vel_subscriber_ = this->private_node_handle_.subscribe("cmd_vel", 1, &SafeCmdAlgNode::cmd_vel_callback,this);
   this->rear_laser_subscriber_ = this->private_node_handle_.subscribe("rear_laser", 1, &SafeCmdAlgNode::rear_laser_callback, this);
   this->front_laser_subscriber_ = this->private_node_handle_.subscribe("front_laser", 1, &SafeCmdAlgNode::front_laser_callback, this);
+  
+  this->rear_laser_watchdog.reset(ros::Duration(this->config.rear_laser_watchdog_time));   
+  this->front_laser_watchdog.reset(ros::Duration(this->config.front_laser_watchdog_time)); 
 
   // read the safety footprint
   this->footprint=costmap_2d::makeFootprintFromParams(this->private_node_handle_);
@@ -51,25 +54,25 @@ void SafeCmdAlgNode::mainNodeThread(void)
     this->new_cmd_vel=false;
     if(this->config.use_front_laser && this->front_laser_watchdog.is_active())
     {
-      ROS_ERROR("SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!");
+      ROS_ERROR_THROTTLE(10,"SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!");
       safe_Twist_msg_.linear.x = 0.0;
       safe_Twist_msg_.linear.y = 0.0;
       safe_Twist_msg_.linear.z = 0.0;
       safe_Twist_msg_.angular.x = 0.0;
       safe_Twist_msg_.angular.y = 0.0;
       safe_Twist_msg_.angular.z = 0.0;
-      cmd_vel_safe_publisher_.publish(Twist_msg_);
+      cmd_vel_safe_publisher_.publish(this->safe_Twist_msg_);
     }
     else if(this->config.use_rear_laser && this->rear_laser_watchdog.is_active())
     {
-      ROS_ERROR("SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!");
+      ROS_ERROR_THROTTLE(10,"SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!");
       safe_Twist_msg_.linear.x = 0.0;
       safe_Twist_msg_.linear.y = 0.0;
       safe_Twist_msg_.linear.z = 0.0;
       safe_Twist_msg_.angular.x = 0.0;
       safe_Twist_msg_.angular.y = 0.0;
       safe_Twist_msg_.angular.z = 0.0;
-      cmd_vel_safe_publisher_.publish(Twist_msg_);
+      cmd_vel_safe_publisher_.publish(this->safe_Twist_msg_);
     }
     else 
     {
@@ -93,6 +96,7 @@ void SafeCmdAlgNode::mainNodeThread(void)
           (*points)=this->points_rear;
         }
       }
+      
       if(new_data)
       {
         std::vector<geometry_msgs::Point> traj_footprint;
@@ -126,7 +130,7 @@ void SafeCmdAlgNode::mainNodeThread(void)
           else if(this->config.architecture==2)
             this->alg_.limit_velocities_differential(this->Twist_msg_.linear.x,this->Twist_msg_.angular.z,this->safe_Twist_msg_.linear.x,this->safe_Twist_msg_.angular.z,collision_index);
           else
-            ROS_WARN("Unknown robot architecture");
+            ROS_ERROR("SafeCmdAlgNode: unknown robot architecture");
         }
         else
           this->safe_Twist_msg_=this->Twist_msg_;
@@ -137,7 +141,10 @@ void SafeCmdAlgNode::mainNodeThread(void)
         this->collision_points_publisher_.publish(this->collision_points_PointCloud2_msg_);
       }
       else
-        this->cmd_vel_safe_publisher_.publish(this->safe_Twist_msg_);
+      {
+        //if no new data, do not publish twist
+        //this->cmd_vel_safe_publisher_.publish(this->safe_Twist_msg_);
+      }
     }
   }
 
@@ -169,7 +176,7 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
     this->alg_.compute_trajectory_differential(msg->linear.x,msg->angular.z,this->traj);
   else
   {
-    ROS_WARN("Unknown robot architecture");
+    ROS_ERROR("SafeCmdAlgNode: unknown robot architecture");
     return;
   }
   this->new_cmd_vel=true;