From ab38865a490e299cbd9a8791ee25d78d8880d215 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Tue, 24 May 2022 12:14:44 +0200
Subject: [PATCH] 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 | 21 ++++++++++++++-------
1 file changed, 14 insertions(+), 7 deletions(-)
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index a26f889..893a89c 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/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;
--
GitLab