Fixed issue with IMU link timestamp

stereolabs · adujardin · Jul 17, 2018 · Jun 5, 2018 · Jun 25, 2018 · Jun 25, 2018
commit a6f4136a07631b44af649d39b2c1f9d086dde0a2
diff --git a/launch/zed_camera.launch b/launch/zed_camera.launch
@@ -41,8 +41,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
     <!-- Publish urdf zed -->
     <arg name="publish_urdf"          default="true" />
+
+    <arg name="rgb_topic"           default="rgb/image_rect_color" />
+    <arg name="rgb_info_topic"      default="rgb/camera_info" />
+    <arg name="depth_topic"         default="depth/depth_registered" />
 
-    <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" output="screen" required="true"><!-- launch-prefix="valgrind" --> 
+    <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" output="screen" required="true"><!-- launch-prefix="valgrind" -->
 
         <param name="camera_model"          value="$(arg camera_model)" />
 
@@ -83,9 +87,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
         <rosparam param="initial_tracking_pose" subst_value="True">$(arg initial_pose)</rosparam>
 
         <!-- ROS topic names -->
-        <param name="rgb_topic"                 value="rgb/image_rect_color" />
+        <param name="rgb_topic"                 value="$(arg rgb_topic)" />
         <param name="rgb_raw_topic"             value="rgb/image_raw_color" />
-        <param name="rgb_cam_info_topic"        value="rgb/camera_info" />
+        <param name="rgb_cam_info_topic"        value="$(arg rgb_info_topic)" />
         <param name="rgb_cam_info_raw_topic"    value="rgb/camera_info_raw" />
 
         <param name="left_topic"                value="left/image_rect_color" />
@@ -98,7 +102,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
         <param name="right_cam_info_topic"      value="right/camera_info" />
         <param name="right_cam_info_raw_topic"  value="right/camera_info_raw" />
 
-        <param name="depth_topic"               value="depth/depth_registered" />
+        <param name="depth_topic"               value="$(arg depth_topic)" />
         <param name="depth_cam_info_topic"      value="depth/camera_info" />
 
         <param name="point_cloud_topic"         value="point_cloud/cloud_registered" />

diff --git a/launch/zed_camera_nodelet.launch b/launch/zed_camera_nodelet.launch
@@ -41,8 +41,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
     <!-- Publish urdf zed -->
     <arg name="publish_urdf"          default="true" />
-
-    <node pkg="nodelet" type="nodelet" name="ZED_nodelet" args="load zed_wrapper/ZEDWrapperNodelet nodelet_manager" output="screen"> 
+
+    <arg name="rgb_topic"           default="rgb/image_rect_color" />
+    <arg name="rgb_info_topic"      default="rgb/camera_info" />
+    <arg name="depth_topic"         default="depth/depth_registered" />
+
+    <node pkg="nodelet" type="nodelet" name="ZED_nodelet" args="load zed_wrapper/ZEDWrapperNodelet nodelet_manager" output="screen">
 
         <param name="camera_model" value="$(arg camera_model)" />
 
@@ -83,37 +87,37 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
         <rosparam param="initial_tracking_pose" subst_value="True">$(arg initial_pose)</rosparam>
 
         <!-- ROS topic names -->
-        <param name="rgb_topic"             value="rgb/image_rect_color" />
-        <param name="rgb_raw_topic"         value="rgb/image_raw_color" />
-        <param name="rgb_cam_info_topic"    value="rgb/camera_info" />
+        <param name="rgb_topic"                 value="$(arg rgb_topic)" />
+        <param name="rgb_raw_topic"             value="rgb/image_raw_color" />
+        <param name="rgb_cam_info_topic"        value="$(arg rgb_info_topic)" />
         <param name="rgb_cam_info_raw_topic"    value="rgb/camera_info_raw" />
 
-        <param name="left_topic"            value="left/image_rect_color" />
-        <param name="left_raw_topic"        value="left/image_raw_color" />
-        <param name="left_cam_info_topic"   value="left/camera_info" />
+        <param name="left_topic"                value="left/image_rect_color" />
+        <param name="left_raw_topic"            value="left/image_raw_color" />
+        <param name="left_cam_info_topic"       value="left/camera_info" />
         <param name="left_cam_info_raw_topic"   value="left/camera_info_raw" />
 
-        <param name="right_topic"           value="right/image_rect_color" />
-        <param name="right_raw_topic"       value="right/image_raw_color" />
-        <param name="right_cam_info_topic"  value="right/camera_info" />
+        <param name="right_topic"               value="right/image_rect_color" />
+        <param name="right_raw_topic"           value="right/image_raw_color" />
+        <param name="right_cam_info_topic"      value="right/camera_info" />
         <param name="right_cam_info_raw_topic"  value="right/camera_info_raw" />
 
-        <param name="depth_topic"           value="depth/depth_registered" />
-        <param name="depth_cam_info_topic"  value="depth/camera_info" />
+        <param name="depth_topic"               value="$(arg depth_topic)" />
+        <param name="depth_cam_info_topic"      value="depth/camera_info" />
 
-        <param name="point_cloud_topic"     value="point_cloud/cloud_registered" />
+        <param name="point_cloud_topic"         value="point_cloud/cloud_registered" />
 
-        <param name="disparity_topic"       value="disparity/disparity_image" />
-        <param name="confidence_img_topic"  value="confidence/confidence_image" />
-        <param name="confidence_map_topic"  value="confidence/confidence_map" />
+        <param name="disparity_topic"           value="disparity/disparity_image" />
+        <param name="confidence_img_topic"      value="confidence/confidence_image" />
+        <param name="confidence_map_topic"      value="confidence/confidence_map" />
 
 
-        <param name="pose_topic"             value="map" />
+        <param name="pose_topic"                value="map" />
 
-        <param name="imu_topic"             value="imu/data" />
-        <param name="imu_topic_raw"         value="imu/data_raw" />
+        <param name="imu_topic"                 value="imu/data" />
+        <param name="imu_topic_raw"             value="imu/data_raw" />
 
-        <param name="imu_pub_rate"          value="100.0" />
+        <param name="imu_pub_rate"              value="500.0" />
 
     </node>
 

diff --git a/src/nodelet/include/zed_wrapper_nodelet.hpp b/src/nodelet/include/zed_wrapper_nodelet.hpp
@@ -108,7 +108,7 @@ namespace zed_wrapper {
          * \param base_transform : Transformation representing the imu pose from camera frame
          * \param t : the ros::Time to stamp the image
          */
-        void publishImuFrame(tf2::Transform base_transform, ros::Time t);
+        void publishImuFrame(tf2::Transform base_transform);
 
         /* \brief Publish a cv::Mat image with a ros Publisher
          * \param img : the image to publish
@@ -282,6 +282,9 @@ namespace zed_wrapper {
         std::string svo_filepath;
         double imu_pub_rate;
 
+        // IMU time
+        ros::Time imu_time;
+
         bool pose_smoothing;
         bool spatial_memory;
 
@@ -291,6 +294,7 @@ namespace zed_wrapper {
         std::vector<float> initial_track_pose;
         tf2::Transform pose_base_transform;
         //tf2::Transform odom_base_transform;
+        tf2::Transform imu_base_transform;
         sl::Transform initial_pose_sl;
 
         // zed object

diff --git a/src/nodelet/src/zed_wrapper_nodelet.cpp b/src/nodelet/src/zed_wrapper_nodelet.cpp
@@ -253,7 +253,6 @@ void ZEDWrapperNodelet::onInit() {
     set_pose( initial_track_pose[0], initial_track_pose[1], initial_track_pose[2],
             initial_track_pose[3], initial_track_pose[4], initial_track_pose[5]);
 
-
     // Initialization transformation listener
     tfBuffer.reset(new tf2_ros::Buffer);
     tf_listener.reset(new tf2_ros::TransformListener(*tfBuffer));
@@ -438,12 +437,13 @@ void ZEDWrapperNodelet::onInit() {
 
     // Imu publisher
     if(imu_pub_rate > 0 && realCamModel == sl::MODEL_ZED_M) {
-        pub_imu = nh.advertise<sensor_msgs::Imu>(imu_topic, 1);
+        pub_imu = nh.advertise<sensor_msgs::Imu>(imu_topic, 500);
         NODELET_INFO_STREAM("Advertized on topic " << imu_topic << " @ " << imu_pub_rate << " Hz");
 
-        pub_imu_raw = nh.advertise<sensor_msgs::Imu>(imu_topic_raw, 1);
+        pub_imu_raw = nh.advertise<sensor_msgs::Imu>(imu_topic_raw, 500);
         NODELET_INFO_STREAM("Advertized on topic " << imu_topic_raw << " @ " << imu_pub_rate << " Hz");
 
+        imu_time = ros::Time::now();
         pub_imu_timer = nh_ns.createTimer(ros::Duration(1.0 / imu_pub_rate), &ZEDWrapperNodelet::imuPubCallback,this);
     } else if(imu_pub_rate > 0 && realCamModel == sl::MODEL_ZED) {
         NODELET_WARN_STREAM( "'imu_pub_rate' set to " << imu_pub_rate << " Hz" << " but ZED camera model does not support IMU data publishing.");
@@ -660,9 +660,9 @@ void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform base_transform, ros::Tim
 //    transform_odom_broadcaster.sendTransform(transformStamped);
 //}
 
-void ZEDWrapperNodelet::publishImuFrame(tf2::Transform base_transform, ros::Time t) {
+void ZEDWrapperNodelet::publishImuFrame(tf2::Transform base_transform) {
     geometry_msgs::TransformStamped transformStamped;
-    transformStamped.header.stamp = t;
+    transformStamped.header.stamp = imu_time;
     transformStamped.header.frame_id = base_frame_id;
     transformStamped.child_frame_id = imu_frame_id;
     // conversion from Tranform to message
@@ -891,8 +891,7 @@ void ZEDWrapperNodelet::dynamicReconfCallback(zed_wrapper::ZedConfig &config, ui
     }
 }
 
-void ZEDWrapperNodelet::imuPubCallback(const ros::TimerEvent & e)
-{
+void ZEDWrapperNodelet::imuPubCallback(const ros::TimerEvent & e) {
     int imu_SubNumber = pub_imu.getNumSubscribers();
     int imu_RawSubNumber = pub_imu_raw.getNumSubscribers();
 
@@ -901,14 +900,14 @@ void ZEDWrapperNodelet::imuPubCallback(const ros::TimerEvent & e)
         return;
     }
 
-    ros::Time t = sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT));
+    ros::Time t = sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_IMAGE));
 
     sl::IMUData imu_data;
     zed.getIMUData(imu_data, sl::TIME_REFERENCE_CURRENT);
 
     if (imu_SubNumber > 0) {
         sensor_msgs::Imu imu_msg;
-        imu_msg.header.stamp = t;
+        imu_msg.header.stamp = imu_time; //t;
         imu_msg.header.frame_id = imu_frame_id;
 
         imu_msg.orientation.x = x_sign * imu_data.getOrientation()[x_idx];
@@ -944,7 +943,7 @@ void ZEDWrapperNodelet::imuPubCallback(const ros::TimerEvent & e)
 
     if (imu_RawSubNumber > 0) {
         sensor_msgs::Imu imu_raw_msg;
-        imu_raw_msg.header.stamp = t;
+        imu_raw_msg.header.stamp = imu_time; //t;
         imu_raw_msg.header.frame_id = imu_frame_id;
 
         imu_raw_msg.angular_velocity.x = x_sign * imu_data.angular_velocity[x_idx];
@@ -1024,24 +1023,18 @@ void ZEDWrapperNodelet::imuPubCallback(const ros::TimerEvent & e)
             base_to_sensor.setIdentity();
         }
 
-        // IMU to base
-        tf2::Transform imu_transform;
-
-        imu_transform = base_to_sensor * imu_sensor * imu_sensor.inverse();
-
-
-        //Note, the frame is published, but its values will only change if someone has subscribed to odom
-        publishImuFrame(imu_transform, t); //publish the tracked Frame
-
+        imu_base_transform = base_to_sensor * imu_sensor * imu_sensor.inverse();
 
+        //Note, the frame is published, but its values will only change if someone has subscribed to IMU
+        publishImuFrame(imu_base_transform); //publish the imu Frame
     }
 }
 
 void ZEDWrapperNodelet::device_poll() {
     ros::Rate loop_rate(rate);
 
-    //ros::Time old_t = ros::Time::now();
     ros::Time old_t = sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT));
+    imu_time = old_t;
 
     sl::ERROR_CODE grab_status;
     tracking_activated = false;
@@ -1105,7 +1098,7 @@ void ZEDWrapperNodelet::device_poll() {
             //ros::Time t = ros::Time::now(); // Get current time
 
             // Timestamp
-            ros::Time t = sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_IMAGE));
+            ros::Time t = sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_IMAGE));            
 
             grabbing = true;
             if (computeDepth) {
@@ -1160,7 +1153,7 @@ void ZEDWrapperNodelet::device_poll() {
             }
 
             // Time update
-            old_t = sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT));
+            old_t = sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT));            
 
             if (autoExposure) {
                 // getCameraSettings() can't check status of auto exposure
@@ -1370,20 +1363,24 @@ void ZEDWrapperNodelet::device_poll() {
             //                publishOdom(odom_base_transform, t);
             //            }
 
-            // Publish odometry tf only if enabled
+            // Publish pose tf only if enabled
             if (publish_tf) {
                 //Note, the frame is published, but its values will only change if someone has subscribed to map
                 publishPoseFrame(pose_base_transform, t);
                 //Note, the frame is published, but its values will only change if someone has subscribed to odom
                 //publishOdomFrame(odom_base_transform, t); //publish the tracked Frame
+                //publishImuFrame(imu_base_transform, t); //publish the imu Frame
+                imu_time = t;
             }
 
             loop_rate.sleep();
         } else {
             // Publish odometry tf only if enabled
             if (publish_tf) {
-                publishPoseFrame(pose_base_transform, sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT)));
-                //publishOdomFrame(odom_base_transform, ros::Time::now()); //publish the tracked Frame before the sleep
+                ros::Time t = sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT));
+                publishPoseFrame(pose_base_transform, t);
+                //publishOdomFrame(odom_base_transform, t); //publish the tracked Frame before the sleep
+                //publishImuFrame(imu_base_transform, t); //publish the imu Frame
             }
             std::this_thread::sleep_for(std::chrono::milliseconds(10)); // No subscribers, we just wait
         }

diff --git a/src/zed_wrapper_node.cpp b/src/zed_wrapper_node.cpp
@@ -24,12 +24,13 @@
 int main(int argc, char** argv) {
     ros::init(argc, argv, "zed_wrapper_node");
 
+    // ZED Nodelet
     nodelet::Loader nodelet;
     nodelet::M_string remap(ros::names::getRemappings());
     nodelet::V_string nargv;
     nodelet.load(ros::this_node::getName(),
-            "zed_wrapper/ZEDWrapperNodelet",
-            remap, nargv);
+                 "zed_wrapper/ZEDWrapperNodelet",
+                 remap, nargv);
 
     ros::spin();