Add tcp_no_delay parameter to sensor models

[efernandez]

This adds the tcp_no_delay ROS parameter to all sensor models. This parameter is equivalent to the ~[sensor]_nodelay parameter in https://github.com/cra-ros-pkg/robot_localization/blob/melodic-devel/doc/state_estimation_nodes.rst#sensor_nodelay.

You can easily see the impact of enabling TCP_NODELAY with rostopic delay --tcpnodelay <topic>, which can be compared against rostopic delay <topic>, e.g.:

$ rostopic delay /odom
subscribed to [/odom]
average delay: 0.015
	min: 0.000s max: 0.043s std dev: 0.01371s window: 49
average delay: 0.017
	min: 0.000s max: 0.043s std dev: 0.01371s window: 100

$ rostopic delay --tcpnodelay /odom
subscribed to [/odom]
average delay: 0.000
	min: 0.000s max: 0.000s std dev: 0.00002s window: 50
average delay: 0.000
	min: 0.000s max: 0.000s std dev: 0.00002s window: 100

I've used this simple Python node to generate dummy messages:

#!/usr/bin/env python3

import rospy

from nav_msgs.msg import Odometry

if __name__ == '__main__':
    rospy.init_node('odometry_publisher')

    try:
        pub = rospy.Publisher('odom', Odometry, queue_size=1)
        msg = Odometry()
        msg.header.frame_id = "odom"
        msg.child_frame_id = "base_link"
        msg.pose.covariance[0] = 1.0e-6
        msg.pose.covariance[7] = 1.0e-6
        msg.pose.covariance[35] = 1.0e-6
        msg.twist.covariance[0] = 1.0e-8
        msg.twist.covariance[7] = 1.0e-8
        msg.twist.covariance[35] = 1.0e-8

        rate = rospy.Rate(50.0)
        while True:
            msg.header.stamp = rospy.get_rostime()
            msg.pose.pose.position.x += 0.1
            msg.twist.twist.linear.x += 0.1 / 50.0
            pub.publish(msg)
            rate.sleep()
    except KeyboardInterrupt:
        rospy.signal_shutdown('Ctrl-c received')

I've also computed the delay in the fuse_models::Odometry2D::process method, with this changes wrt this PR:

$ git diff HEAD~1
diff --git a/fuse_models/include/fuse_models/odometry_2d.h b/fuse_models/include/fuse_models/odometry_2d.h
index 061d361..dca1807 100644
--- a/fuse_models/include/fuse_models/odometry_2d.h
+++ b/fuse_models/include/fuse_models/odometry_2d.h
@@ -133,6 +133,8 @@ protected:
 
   ros::Subscriber subscriber_;
 
+  ros::Publisher publisher_;
+
   using OdometryThrottledCallback = fuse_core::ThrottledMessageCallback<nav_msgs::Odometry>;
   OdometryThrottledCallback throttled_callback_;
 };
diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp
index 7d159fb..eaee7dd 100644
--- a/fuse_models/src/odometry_2d.cpp
+++ b/fuse_models/src/odometry_2d.cpp
@@ -92,6 +92,8 @@ void Odometry2D::onStart()
     subscriber_ = node_handle_.subscribe<nav_msgs::Odometry>(ros::names::resolve(params_.topic), params_.queue_size,
                                                              &OdometryThrottledCallback::callback, &throttled_callback_,
                                                              ros::TransportHints().tcpNoDelay(params_.tcp_no_delay));
+
+    publisher_ = node_handle_.advertise<nav_msgs::Odometry>(name_ + "/delay", 10);
   }
 }
 
@@ -102,6 +104,14 @@ void Odometry2D::onStop()
 
 void Odometry2D::process(const nav_msgs::Odometry::ConstPtr& msg)
 {
+  const auto delay = (ros::Time::now() - msg->header.stamp).toSec();
+
+  nav_msgs::Odometry delay_msg;
+  delay_msg.header = msg->header;
+  delay_msg.pose.pose.position.x = delay;
+
+  publisher_.publish(delay_msg);
+
   // Create a transaction object
   auto transaction = fuse_core::Transaction::make_shared();
   transaction->stamp(msg->header.stamp);

With this I can easily plot the delay and see the impact of TCP_NODELAY:

• tcp_no_delay: false (default):
  

• tcp_no_delay: true:
  

The next plot shows the delay for tcp_no_delay: true and tcp_no_delay: false one after the other, so it's easier to see the impact, with the same axes scale:

We could do the same in simulation, but in that case the delay is computed using the simulated time, not the wall time, so there are some discretization artifacts, e.g. with simulation running at 100Hz:

This also allows to see that for nav_msgs::Odometry messages, Nagle's algorithm buffers approximately 11-12 messages.

More information on TCP_NODELAY and other related topics can be found in:
https://www.extrahop.com/company/blog/2016/tcp-nodelay-nagle-quickack-best-practices/

[svwilliams]

This is great. Thanks.

[efernandez]

A big part of the merit should go to @ayrton04 , who already exposed this option in robot_localization. 😃

I just took the time to confirm it's actually reducing the delay for the small messages the sensor models subscribe to.
BTW, I also tried evaluating things on bagged data, but that didn't shouw any difference. Part of the reason is that rosbag record doesn't use TCP_NODELAY when recording 😉 , and I believe rosbag play provides a simulated clock that somehow interfered with my comparison. Just saying, in case you want to confirm my results yourself.
I ran my tests on a beefy desktop computer, so I expect your results to be the same or worse. And I guess they'd be similar if you run things live on a robot running ROS, likely with hundreds of topics / sockets. TBH I don't know if that matters, but I think it's worth trying and documenting it.

[svwilliams]

Never give @ayrton04 credit. It goes straight to his head. 😄