From dc5b5d526240d864dbf0efb0f76cb69e4254daf9 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 24 Dec 2021 12:19:20 +0100
Subject: [PATCH] moved from setLastStamp to updateLastHeader to check seq as
well
---
include/subscriber.h | 21 ++++++++++++++++-----
src/subscriber_diffdrive.cpp | 2 +-
src/subscriber_odom2d.cpp | 2 +-
3 files changed, 18 insertions(+), 7 deletions(-)
diff --git a/include/subscriber.h b/include/subscriber.h
index e93fe68..885d400 100644
--- a/include/subscriber.h
+++ b/include/subscriber.h
@@ -72,6 +72,7 @@ class Subscriber
// ros
ros::Subscriber sub_;
ros::Time last_stamp_;
+ int last_seq_;
public:
Subscriber(const std::string& _unique_name,
@@ -80,7 +81,8 @@ class Subscriber
sensor_ptr_(_sensor_ptr),
prefix_("ROS subscriber/" + _unique_name),
name_(_unique_name),
- last_stamp_(0)
+ last_stamp_(0),
+ last_seq_(-1)
{
topic_ = _server.getParam<std::string>(prefix_ + "/topic");
}
@@ -99,7 +101,7 @@ class Subscriber
protected:
- void setLastStamp(const ros::Time _stamp);
+ void updateLastHeader(const std_msgs::Header& _header);
template<typename T>
T getParamWithDefault(const ParamsServer &_server,
@@ -126,15 +128,24 @@ inline double Subscriber::secondsSinceLastCallback()
{
if (last_stamp_ == ros::Time(0))
{
- WOLF_WARN("Subscriber: 'last_stamp_` not initialized. No messages have been received or ", name_, " is not updating this attribute.");
+ WOLF_WARN("Subscriber: 'last_stamp_` not initialized. No messages have been received or ", name_, " is not updating headers (be sure to add 'updateLastHeader(msg->header)' in your subscriber callback).");
return 0;
}
return (ros::Time::now() - last_stamp_).toSec();
}
-inline void Subscriber::setLastStamp(const ros::Time _stamp)
+inline void Subscriber::updateLastHeader(const std_msgs::Header& _header)
{
- last_stamp_ = _stamp;
+ // stamp
+ if ((ros::Time::now() - _header.stamp).toSec() > 1) // in case use_sim_time == false
+ last_stamp_ = ros::Time::now();
+ else
+ last_stamp_ = _header.stamp;
+
+ // seq
+ if (last_seq_ >= 0 and _header.seq - last_seq_ > 1)
+ ROS_ERROR("Subscriber %s lost %i messages!", name_.c_str(), _header.seq - last_seq_ - 1);
+ last_seq_ = _header.seq;
}
template<typename T>
diff --git a/src/subscriber_diffdrive.cpp b/src/subscriber_diffdrive.cpp
index 2b2f0a5..51c5a2d 100644
--- a/src/subscriber_diffdrive.cpp
+++ b/src/subscriber_diffdrive.cpp
@@ -48,7 +48,7 @@ void SubscriberDiffdrive::initialize(ros::NodeHandle& nh, const std::string& top
void SubscriberDiffdrive::callback(const sensor_msgs::JointState::ConstPtr& msg)
{
- setLastStamp(msg->header.stamp);
+ updateLastHeader(msg->header);
auto left_angle = msg->position[0];
auto right_angle = msg->position[1];
diff --git a/src/subscriber_odom2d.cpp b/src/subscriber_odom2d.cpp
index baca436..1c8396f 100644
--- a/src/subscriber_odom2d.cpp
+++ b/src/subscriber_odom2d.cpp
@@ -63,7 +63,7 @@ void SubscriberOdom2d::callback(const nav_msgs::Odometry::ConstPtr& msg)
{
ROS_DEBUG("WolfNodePolyline::odomCallback");
- setLastStamp(msg->header.stamp);
+ updateLastHeader(msg->header);
if (last_odom_stamp_ != ros::Time(0))
{
--
GitLab