Why does ROS overwrite my sequence number?
I have noticed that the ROS Publishers seem to overwrite the sequence number set by me in the message header (std_msgs/Header
). This only happens if the message is one known by ROS (such as sensor_msgs/Image
) but not if it is one I defined myself.
What happens is that I set the header.seq field in e.g. the sensor_msgs/Image
message to something, but during publishing, ROS overwrites this number with a counter which is apparently kept in the ros::Publisher
class itself. So, when I subscribe to the message, sequence numbers of sensor_msgs/Image
always start with 0 and count up by one from here, no matter what I set in the header before my call to ros::Publisher::publish()
.
I dug a little deeper and found that I can prevent this by using ros::AdvertiseOptions
.
ros::NodeHandle n;
ros::AdvertizeOptions op = ros::AdvertiseOptions::create<sensor_msgs::Image>("/foo", 1, &connected, &disconnected, ros::VoidPtr(), NULL);
op.has_header = false;
ros::Publisher pub = n.advertise(op);
The callbacks unfortunately have to be defined, I was unable to just give NULL
...
void connected(const ros::SingleSubscriberPublisher&) {}
void disconnected(const ros::SingleSubscriberPublisher&) {}
This way, my sequence numbers persist and are not overwritten by the ros::Publisher::publish()
call later.
My question is, why does ROS not trust me to set the sequence number as I want it?
My test publisher:
#include <stdio.h> #include <stdlib.h> #include <time.h> #include <ros ros.h=""> #include <sensor_msgs image.h=""> // dummy callbacks void connected(const ros::SingleSubscriberPublisher&) {} void disconnected(const ros::SingleSubscriberPublisher&) {} int main(int argc, char** argv) { srand ( time(NULL) ); ros::init(argc, argv, "test_pub"); ros::NodeHandle n; ros::Publisher pub_messed_up = n.advertise<sensor_msgs::image>("/foo", 1); ros::AdvertiseOptions op = ros::AdvertiseOptions::create<sensor_msgs::image>("/bar", 1, &connected, &disconnected, ros::VoidPtr(), NULL); op.has_header = false; ros::Publisher pub_ok = n.advertise(op); ros::Rate r(1); while(ros::ok()) { sensor_msgs::Image msg; msg.header.seq = rand(); msg.header.stamp = ros::Time::now(); msg.header.frame_id = "foo"; msg.height = msg.header.seq; pub_messed_up.publish(msg); pub_ok.publish(msg); r.sleep(); } }
My test subscriber:
#include <ros ros.h=""> #include <sensor_msgs image.h=""> void callback( const sensor_msgs::ImageConstPtr& msg ) { ROS_INFO("BAD: image seq is: %10u, should be %10u", msg->header.seq, msg->height); } void callback2( const sensor_msgs::ImageConstPtr& msg ) { ROS_INFO("OK : image seq is: %10u, should be %10u", msg->header.seq, msg->height); } int main(int argc, char** argv) { ros::init(argc, argv, "test_sub"); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("/foo", 1, &callback); ros::Subscriber sub2 = n.subscribe("/bar", 1, &callback2); ros::spin(); }
Output:
[ INFO] [1360774481.858622442]: OK : image seq is: 180403487, should be 180403487 [ INFO] [1360774481.858737733]: BAD: image seq is: 3, should be 180403487 [ INFO] [1360774481.958441959]: OK : image seq is: 1993875195, should be 1993875195 [ INFO] [1360774481.958503343]: BAD: image seq is: 4, should be 1993875195 [ INFO] [1360774482.058392020]: OK : image seq is: 458683548, should be 458683548 [ INFO] [1360774482.058461137]: BAD: image seq is: 5, should be 458683548 [ INFO] [1360774482.158405818]: OK : image seq ...