Optionally publish a sim clock and use timestamps stored in svo in message headers rather than current wall time.

Sim clock appears to be working, may have some odd glitches, may mess up non svo-playback modes

Adjusting some of the timestamps, not sure if all pubs are using good timestamps, some of the static frames are timestamp 0 (but that's okay for static frames?), need to figure out if faster than real time playback is possible- there must be an api that allows read frames quickly, but maybe ruled out here.

Author: lucasw

zed_nodelets/CMakeLists.txt

@@ -42,6 +42,7 @@ find_package(catkin REQUIRED COMPONENTS
     roscpp
     image_transport
     rosconsole
+    rosgraph_msgs
     sensor_msgs
     stereo_msgs
     std_msgs

zed_nodelets/package.xml

@@ -18,6 +18,7 @@
     <depend>nav_msgs</depend>
     <depend>roscpp</depend>
     <depend>rosconsole</depend>
+    <depend>rosgraph_msgs</depend>
     <depend>sensor_msgs</depend>
     <depend>stereo_msgs</depend>
     <depend>message_filters</depend>

zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp

@@ -31,6 +31,7 @@
 #include <image_transport/image_transport.h>
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
+#include <rosgraph_msgs/Clock.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <tf2/LinearMath/Transform.h>
 #include <tf2_ros/static_transform_broadcaster.h>
@@ -117,6 +118,17 @@ class ZEDWrapperNodelet : public nodelet::Nodelet
    */
   void readParameters();
 
+  ros::WallTimer wall_timer_;
+  ros::Time sim_clock_base_time;
+  /*! \brief Publish the current time when use_sim_time is true
+   * \param t : the ros::Time to send in the clock message
+   */
+  void publishSimClock(const ros::Time& stamp);
+
+  /*! \brief sim clock update thread
+   */
+  void sim_clock_update(const ros::WallTimerEvent& e);
+
   /*! \brief ZED camera polling thread function
    */
   void device_poll_thread_func();
@@ -156,7 +168,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet
   /*!
    * \brief Publish IMU frame once as static TF
    */
-  void publishStaticImuFrame();
+  void publishStaticImuFrame(const ros::Time& t);
 
   /*! \brief Publish a sl::Mat image with a ros Publisher
    * \param imgMsgPtr : the image message to publish
@@ -417,6 +429,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet
   image_transport::CameraPublisher mPubRightGray;
   image_transport::CameraPublisher mPubRawRightGray;
 
+  ros::Publisher mPubSimClock;
+
   ros::Publisher mPubConfMap;    //
   ros::Publisher mPubDisparity;  //
   ros::Publisher mPubCloud;
@@ -610,6 +624,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet
   double mCamDepthResizeFactor = 1.0;
 
   // flags  
+  bool mUseSimTime = false;
   bool mTriggerAutoExposure = true;
   bool mTriggerAutoWB = true;
   bool mComputeDepth;