Added diagnostics

CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm message_generation nav_msgs geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm message_generation nav_msgs geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs iri_ros_tools)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -61,7 +61,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm message_runtime nav_msgs geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs
+ CATKIN_DEPENDS iri_base_algorithm message_runtime nav_msgs geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs iri_ros_tools
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 

cfg/CollisionManager.cfg

@@ -47,6 +47,7 @@ gen.add("fixed_frame",                          str_t,      0,           "Fixed
 gen.add("base_link_frame",                      str_t,      0,           "Base_link frame",                                       "base_link")
 gen.add("tf_timeout",                           double_t,   0,           "Timeout to find a transform",                           0.2, 0.1, 2.0)
 gen.add("calculate_ang_vel",                    bool_t,     0,           "Flag to calculate robot angular vel from odometry",     False)
+gen.add("watchdog_t",                           double_t,   0,           "Maximum time between subscribers msgs",                 1.0,   0.1, 10.0)
 
 collision_detection.add("collision_timeout",                        double_t,   0,           "Timeout to detect an end of collision",                            2.0, 1.0, 30.0)
 collision_detection.add("collision_acc_transition_counter_en",      bool_t,     0,           "Enable the collision counter for the acc detection",               False)

config/analyzers.yaml

+pub_rate: 1.0
+analyzers:
+  collision_detection:
+    type: diagnostic_aggregator/GenericAnalyzer
+    path: collision_detection
+    contains: [
+      'imu',
+      'odom']
+  ICP_check:
+    type: diagnostic_aggregator/GenericAnalyzer
+    path: icp_check
+    contains: [
+      'odom',
+      'laser']

config/params.yaml

@@ -5,6 +5,7 @@ base_link_frame: base_link
 tf_timeout: 0.2
 err_msg_rate: 0.5
 calculate_ang_vel: False
+watchdog_t: 1.0
 
 collision_timeout: 4.0
 collision_acc_transition_counter_en: True

include/collision_manager_alg_node.h

@@ -29,6 +29,7 @@
 #include "collision_manager_alg.h"
 #include <Eigen/Eigen>
 #include <tf2/utils.h>
+#include <iri_ros_tools/watchdog.h>
 
 // [publisher subscriber headers]
 #include <iri_collision_manager/collision.h>
@@ -45,6 +46,10 @@
 
 // [action server client headers]
 
+// diagnostics
+#include "diagnostic_updater/diagnostic_updater.h"
+#include "diagnostic_updater/update_functions.h"
+
 /**
  * \struct RangesWithStamp.
  *
@@ -107,6 +112,10 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     Eigen::Vector3d odom_before_collision_; ///< Odometry data before collision.
     Eigen::Vector3d odom_after_collision_; ///< Odometry data after collision.
     bool odom_input_is_ok_; ///< Bool to know if the wheel odometry from a collision is ok.
+    CROSWatchdog imu_watchdog_; ///< IMU callback watchdog.
+    CROSWatchdog odom_watchdog_; ///< Odom callback watchdog.
+    CROSWatchdog front_laser_watchdog_; ///< Front laser callback watchdog.
+    CROSWatchdog rear_laser_watchdog_; ///< Rear laser callback watchdog.
 
     // [publisher attributes]
     ros::Publisher collisions_publisher_;
@@ -182,6 +191,19 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
 
     // [action client attributes]
 
+
+    // Diagnostics
+    diagnostic_updater::Heartbeat heart_beat_;
+    diagnostic_updater::FunctionDiagnosticTask imu_diag_;
+    diagnostic_updater::FunctionDiagnosticTask odom_diag_;
+    diagnostic_updater::FunctionDiagnosticTask front_laser_diag_;
+    diagnostic_updater::FunctionDiagnosticTask rear_laser_diag_;
+
+    void check_imu_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
+    void check_odom_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
+    void check_front_laser_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
+    void check_rear_laser_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
+
    /**
     * \brief config variable
     *

launch/collisions.launch

@@ -7,6 +7,7 @@
   <arg name="sim_time"      default="false"/>
   <param name="/use_sim_time" value="$(arg sim_time)" />
   <arg name="imu_driver"    default="true"/>
+  <arg name="diagnostics"   default="true"/>
 
   <!--  IMU driver parameters  -->
   <arg name="imu_config_file"      default="$(find iri_collision_manager)/config/bno055.yaml" />
@@ -46,6 +47,7 @@
     <arg name="imu_topic_out" value="$(arg imu_filtered_topic)"/>
     <arg name="odom_topic" value="$(arg odom_topic)"/>
     <arg name="update_bias_ns" value="$(arg update_bias_ns)"/>
+    <arg name="diagnostics" value="$(arg diagnostics)"/>
   </include>
 
   <include file="$(find iri_laser_scan_icp)/launch/node.launch">
@@ -66,6 +68,7 @@
     <arg name="rear_laser_topic" value="$(arg rear_laser_topic)"/>
     <arg name="icp_service_name" value="$(arg icp_service_name)"/>
     <arg name="odom_topic" value="$(arg odom_topic)"/>
+    <arg name="diagnostics" value="$(arg diagnostics)"/>
   </include>
 
 

launch/node.launch

@@ -5,6 +5,8 @@
   <arg name="output"        default="screen"/>
   <arg name="launch_prefix" default=""/>
   <arg name="config_file"   default="$(find iri_collision_manager)/config/params.yaml"/>
+  <arg name="diagnostics"   default="true"/>
+
   <!-- <arg name="topic_name"  default="new_topic_name"/> -->
   <arg name="imu_topic"     default="/bno055_imu/imu"/>
   <arg name="front_laser_topic"     default="~/front_laser_scan"/>
@@ -26,4 +28,13 @@
     <remap from="~/odom" to="$(arg odom_topic)"/>
   </node>
 
+  <node pkg="diagnostic_aggregator" 
+        type="aggregator_node"
+        name="collision_manager_diagnostic_aggregator" 
+        if  ="$(arg diagnostics)">
+    <!-- Load the file you made above -->
+    <rosparam command="load" 
+              file="$(find iri_collision_manager)/config/analyzers.yaml" />
+  </node>
+
 </launch>

launch/test.launch

@@ -5,6 +5,8 @@
   <arg name="launch_prefix" default=""/>
   <arg name="dr"            default="false"/>
   <arg name="collision_config_file"   default="$(find iri_collision_manager)/config/params.yaml"/>
+  <arg name="diagnostics"   default="false"/>
+
   <arg name="imu_topic"     default="/helena/sensors/bno055_imu/imu"/>
 
   <arg name="sim_time"              default="false"/>
@@ -20,6 +22,7 @@
     <arg name="rear_laser_topic" value="/rear_laser_scan"/>
     <arg name="icp_service_name" value="~/icp"/>
     <arg name="odom_topic" value="/odom"/>
+    <arg name="diagnostics" value="$(arg diagnostics)"/>
   </include>
 
   <node name="rqt_reconfigure_iri_collision_manager"

package.xml

@@ -61,6 +61,7 @@
   <depend>sensor_msgs</depend>
   <depend>iri_bno055_imu_bringup</depend>
   <depend>iri_bno055_imu_driver</depend>
+  <depend>iri_ros_tools</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/collision_manager_alg_node.cpp

@@ -2,8 +2,12 @@
 #include <math.h>  
 
 CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
-  algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm>()
-  ,tf2_listener(tf2_buffer)
+  algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm>(),
+  imu_diag_("imu",boost::bind(&CollisionManagerAlgNode::check_imu_status, this, _1)),
+  odom_diag_("odom",boost::bind(&CollisionManagerAlgNode::check_odom_status, this, _1)),
+  front_laser_diag_("front_laser",boost::bind(&CollisionManagerAlgNode::check_front_laser_status, this, _1)),
+  rear_laser_diag_("rear_laser",boost::bind(&CollisionManagerAlgNode::check_rear_laser_status, this, _1)),
+  tf2_listener(tf2_buffer)
 {
   //init class attributes if necessary
   if(!this->private_node_handle_.getParam("rate", this->config_.rate))
@@ -15,16 +19,48 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
 
   if(!this->private_node_handle_.getParam("base_link_frame", this->config_.base_link_frame))
   {
-    ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'base_link' not found. Setting it to base_link.");
+    ROS_WARN("CollisionManagerAlgNode::CollisionManagerAlgNode: param 'base_link' not found. Setting it to base_link.");
     this->config_.base_link_frame = "base_link";
   }
 
   if(!this->private_node_handle_.getParam("fixed_frame", this->config_.fixed_frame))
   {
-    ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'base_link' not found. Setting it to map.");
+    ROS_WARN("CollisionManagerAlgNode::CollisionManagerAlgNode: param 'base_link' not found. Setting it to map.");
     this->config_.fixed_frame = "map";
   }
 
+  if(!this->private_node_handle_.getParam("front_laser_en", this->config_.front_laser_en))
+  {
+    ROS_WARN("CollisionManagerAlgNode::CollisionManagerAlgNode: param 'front_laser_en' not found. Setting it to true.");
+    this->config_.front_laser_en = true;
+  }
+
+  if(!this->private_node_handle_.getParam("rear_laser_en", this->config_.rear_laser_en))
+  {
+    ROS_WARN("CollisionManagerAlgNode::CollisionManagerAlgNode: param 'rear_laser_en' not found. Setting it to true.");
+    this->config_.rear_laser_en = true;
+  }
+
+  if(!this->private_node_handle_.getParam("watchdog_t", this->config_.watchdog_t))
+  {
+    ROS_WARN("CollisionManagerAlgNode::CollisionManagerAlgNode: param 'watchdog_t' not found. Configuring wathcdogs with 1.0 sec");
+    this->odom_watchdog_.reset(ros::Duration(1.0));
+    this->imu_watchdog_.reset(ros::Duration(1.0));
+    if (this->config_.front_laser_en)
+      this->front_laser_watchdog_.reset(ros::Duration(1.0));
+    if (this->config_.rear_laser_en)
+      this->rear_laser_watchdog_.reset(ros::Duration(1.0));
+  }
+  else
+  {
+    this->odom_watchdog_.reset(ros::Duration(this->config_.watchdog_t));
+    this->imu_watchdog_.reset(ros::Duration(this->config_.watchdog_t));
+    if (this->config_.front_laser_en)
+      this->front_laser_watchdog_.reset(ros::Duration(this->config_.watchdog_t));
+    if (this->config_.rear_laser_en)
+      this->rear_laser_watchdog_.reset(ros::Duration(this->config_.watchdog_t));
+  }
+
   this->new_imu_input_ = false;
   this->imu_frame_ = "";
   this->front_laser_frame_ = "";
@@ -617,6 +653,13 @@ bool CollisionManagerAlgNode::check_ang_vel_collision_transition(double _ang_vel
   double diff = std::fabs(_ang_vel - this->angular_vel_);
   if (!this->config_.collision_ang_vel_transition_counter_en)
     return true;
+
+  if (this->odom_watchdog_.is_active())
+  {
+    ROS_WARN("CollisionManagerAlgNode::check_ang_vel_collision_transition -> Odom watchdog is active. Angular velocity collision detection can't be performed.");
+    return false;
+  }
+
   if ((_start && (diff > this->config_.collision_ang_vel_th)) || (!_start && (diff < this->config_.collision_ang_vel_th)))
     this->low_ang_vel_count_++;
   else
@@ -664,6 +707,13 @@ bool CollisionManagerAlgNode::set_ref_ranges(bool _front)
 {
   if (!(_front? this->config_.front_laser_en: this->config_.rear_laser_en))
     return false;
+
+  if ((_front? this->front_laser_watchdog_: this->rear_laser_watchdog_).is_active())
+  {
+    ROS_WARN_STREAM("CollisionManagerAlgNode::set_ref_ranges -> " << (_front? "Front ": "Rear ") << "laser watchdog is active. This laser can't be used to perform an ICP.");
+    return false;
+  }
+
   auto it = (_front? this->front_ranges_.begin(): this->rear_ranges_.begin());
   while ((it != (_front? this->front_ranges_.end(): this->rear_ranges_.end())) && (it->stamp > (this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision))))
     ++it;
@@ -688,6 +738,12 @@ bool CollisionManagerAlgNode::set_ref_ranges(bool _front)
 
 bool CollisionManagerAlgNode::set_odom_before_collision(void)
 {
+  if (this->odom_watchdog_.is_active())
+  {
+    ROS_WARN_STREAM("CollisionManagerAlgNode::set_odom_before_collision -> Odom watchdog is active. ICP odometry check can't be performed.");
+    return false;
+  }
+
   auto it = this->odom_buffer_.begin();
   while ((it != this->odom_buffer_.end()) && (it->stamp > (this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision))))
     ++it;
@@ -709,6 +765,12 @@ bool CollisionManagerAlgNode::set_last_ranges(bool _front)
   if (!(_front? this->config_.front_laser_en: this->config_.rear_laser_en))
     return false;
 
+  if ((_front? this->front_laser_watchdog_: this->rear_laser_watchdog_).is_active())
+  {
+    ROS_WARN_STREAM("CollisionManagerAlgNode::set_last_ranges -> " << (_front? "Front ": "Rear ") << "laser watchdog is active. This laser can't be used to perform an ICP.");
+    return false;
+  }
+
   if ((_front? this->front_ranges_: this->rear_ranges_).size() > 0)
   {
     (_front? this->front_icp_srv_: this->rear_icp_srv_).request.last_ranges = (_front? this->front_ranges_: this->rear_ranges_).front().ranges;
@@ -727,6 +789,12 @@ bool CollisionManagerAlgNode::set_last_ranges(bool _front)
 
 bool CollisionManagerAlgNode::get_last_odom(void)
 {
+  if (this->odom_watchdog_.is_active())
+  {
+    ROS_WARN_STREAM("CollisionManagerAlgNode::get_last_odom -> Odom watchdog is active. ICP odometry check can't be performed.");
+    return false;
+  }
+
   if (this->odom_buffer_.size() > 0)
   {
     this->odom_after_collision_ = this->odom_buffer_.front().odom;
@@ -897,9 +965,16 @@ bool CollisionManagerAlgNode::load_tf_blink_laser(bool _front)
 
 bool CollisionManagerAlgNode::check_odometry_diff(const Eigen::Vector3d& _icp_odometry, Eigen::Vector3d& _icp_odometry_diff)
 {
+  if (this->odom_watchdog_.is_active())
+  {
+    ROS_WARN_STREAM("CollisionManagerAlgNode::check_odometry_diff -> Odom watchdog is active. ICP odometry check can't be performed.");
+    _icp_odometry_diff << 0.0, 0.0, 0.0;
+    return false;
+  }
+
   if (!this->odom_input_is_ok_)
   {
-    ROS_WARN_STREAM("CollisionManagerAlgNode::check_odometry_diff -> No wheel odometry information has been saved. Odometry diff can't be calculated.");
+    ROS_WARN_STREAM("CollisionManagerAlgNode::check_odometry_diff -> No wheel odometry information has been saved. ICO odometry difference can't be calculated.");
     _icp_odometry_diff << 0.0, 0.0, 0.0;
     return false;
   }
@@ -954,6 +1029,7 @@ void CollisionManagerAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr&
     this->odom_buffer_.pop_back();
   this->odom_buffer_.push_front(odom);
   this->odom_frame_ = msg->header.frame_id;
+  this->odom_watchdog_.reset(ros::Duration(this->config_.watchdog_t));
 
   //std::cout << msg->data << std::endl;
   //unlock previously blocked shared variables
@@ -1014,6 +1090,7 @@ void CollisionManagerAlgNode::rear_laser_scan_callback(const sensor_msgs::LaserS
     if (this->rear_ranges_.size() >= this->config_.buffer_size)
       this->rear_ranges_.pop_back();
     this->rear_ranges_.push_front(r);
+    this->rear_laser_watchdog_.reset(ros::Duration(this->config_.watchdog_t));
   }
   //std::cout << msg->data << std::endl;
   //unlock previously blocked shared variables
@@ -1074,6 +1151,7 @@ void CollisionManagerAlgNode::front_laser_scan_callback(const sensor_msgs::Laser
     if (this->front_ranges_.size() >= this->config_.buffer_size)
       this->front_ranges_.pop_back();
     this->front_ranges_.push_front(r);
+    this->front_laser_watchdog_.reset(ros::Duration(this->config_.watchdog_t));
   }
 
   //std::cout << msg->data << std::endl;
@@ -1104,6 +1182,7 @@ void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg
   this->imu_local_acc_ = Eigen::Vector3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);
   this->imu_local_ang_vel_ = Eigen::Vector3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);
   this->imu_stamp_ = msg->header.stamp;
+  this->imu_watchdog_.reset(ros::Duration(this->config_.watchdog_t));
   // if (!this->in_collision_)
   //   this->collision_start_stamp_ = msg->header.stamp;
   // if (this->in_collision_)
@@ -1131,6 +1210,52 @@ void CollisionManagerAlgNode::imu_mutex_exit(void)
 
 /*  [action requests] */
 
+void CollisionManagerAlgNode::check_imu_status(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+  if (this->imu_watchdog_.is_active())
+  {
+    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Imu watchdog is active. No message has been received in a while. No collision detection can be performed");
+    ROS_ERROR_STREAM("CollisionManagerAlgNode::check_imu_status -> No imu msgs received in " << this->config_.watchdog_t << " seconds.");
+    ROS_ERROR("CollisionManagerAlgNode::check_imu_status");
+  }
+  else
+    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Imu msg received.");
+}
+
+void CollisionManagerAlgNode::check_odom_status(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+  if (this->odom_watchdog_.is_active())
+    stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Odom watchdog is active. No message has been received in a while. Collision detection from angular velocity can't be performed. ICP odometry check can't be performed.");
+  else
+    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Odom msg received.");
+}
+
+void CollisionManagerAlgNode::check_front_laser_status(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+  if (this->config_.front_laser_en)
+  {
+    if (this->front_laser_watchdog_.is_active())
+      stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Front laser watchdog is active. No message has been received in a while. ICP can't be performed.");
+    else
+      stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Front_laser msg received.");
+  }
+  else
+    stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Front laser not enabled.");
+}
+
+void CollisionManagerAlgNode::check_rear_laser_status(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+  if (this->config_.rear_laser_en)
+  {
+    if (this->rear_laser_watchdog_.is_active())
+      stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Rear laser watchdog is active. No message has been received in a while. ICP can't be performed.");
+    else
+      stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Rear laser msg received.");
+  }
+  else
+    stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Rear laser not enabled.");
+}
+
 void CollisionManagerAlgNode::node_config_update(Config &config, uint32_t level)
 {
   this->alg_.lock();
@@ -1138,6 +1263,11 @@ void CollisionManagerAlgNode::node_config_update(Config &config, uint32_t level)
   this->rear_laser_scan_mutex_enter();
   this->front_laser_scan_mutex_enter();
 
+  if (config.front_laser_en && !this->config_.front_laser_en)
+    this->front_laser_watchdog_.reset(ros::Duration(1.0));
+  if (config.rear_laser_en && !this->config_.rear_laser_en)
+    this->rear_laser_watchdog_.reset(ros::Duration(1.0));
+
   if(config.rate!=this->getRate())
     this->setRate(config.rate);
   this->config_=config;
@@ -1150,6 +1280,11 @@ void CollisionManagerAlgNode::node_config_update(Config &config, uint32_t level)
 
 void CollisionManagerAlgNode::addNodeDiagnostics(void)
 {
+  this->diagnostic_.add(this->heart_beat_);
+  this->diagnostic_.add(this->imu_diag_);
+  this->diagnostic_.add(this->odom_diag_);
+  this->diagnostic_.add(this->front_laser_diag_);
+  this->diagnostic_.add(this->rear_laser_diag_);
 }
 
 /* main function */