Fixes to odometry TF

 * added missing "publish_map_tf" param to launch files

zed_display_rviz/rviz/zedm.rviz

@@ -11,11 +11,9 @@ Panels:
         - /Video1/Camera view1/Visibility1
         - /Depth1/DepthCloud1
         - /Pose1
-        - /Pose1/Odometry1
         - /Pose1/Odometry1/Covariance1
         - /Pose1/Odometry1/Covariance1/Position1
         - /Pose1/Odometry1/Covariance1/Orientation1
-        - /Pose1/Pose1
         - /Pose1/PoseWithCovariance1
         - /Pose1/PoseWithCovariance1/Covariance1
         - /Pose1/PoseWithCovariance1/Covariance1/Position1
@@ -110,8 +108,6 @@ Visualization Manager:
         All Enabled: false
         imu_link:
           Value: true
-        map:
-          Value: true
         odom:
           Value: true
         zed_camera_center:
@@ -130,17 +126,16 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        map:
-          odom:
-            zed_camera_center:
-              imu_link:
+        odom:
+          zed_camera_center:
+            imu_link:
+              {}
+            zed_left_camera_frame:
+              zed_left_camera_optical_frame:
+                {}
+            zed_right_camera_frame:
+              zed_right_camera_optical_frame:
                 {}
-              zed_left_camera_frame:
-                zed_left_camera_optical_frame:
-                  {}
-              zed_right_camera_frame:
-                zed_right_camera_optical_frame:
-                  {}
       Update Interval: 0
       Value: true
     - Class: rviz/Group
@@ -320,7 +315,7 @@ Visualization Manager:
           Buffer Length: 1
           Class: rviz/Path
           Color: 255; 25; 0
-          Enabled: false
+          Enabled: true
           Head Diameter: 0.0299999993
           Head Length: 0.0199999996
           Length: 0.300000012
@@ -338,7 +333,7 @@ Visualization Manager:
           Shaft Length: 0.0500000007
           Topic: /zed/path_odom
           Unreliable: true
-          Value: false
+          Value: true
         - Angle Tolerance: 0
           Class: rviz/Odometry
           Covariance:
@@ -377,7 +372,7 @@ Visualization Manager:
           Buffer Length: 1
           Class: rviz/Path
           Color: 25; 255; 0
-          Enabled: false
+          Enabled: true
           Head Diameter: 0.0299999993
           Head Length: 0.0199999996
           Length: 0.300000012
@@ -395,7 +390,7 @@ Visualization Manager:
           Shaft Length: 0.0500000007
           Topic: /zed/path_map
           Unreliable: true
-          Value: false
+          Value: true
         - Alpha: 1
           Axes Length: 1
           Axes Radius: 0.100000001
@@ -470,7 +465,7 @@ Visualization Manager:
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: map
+    Fixed Frame: odom
     Frame Rate: 60
   Name: root
   Tools:
@@ -491,25 +486,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.48717809
+      Distance: 1.64165437
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.149202153
-        Y: -0.131090492
-        Z: 0.699279904
+        X: -0.146052256
+        Y: -0.190351665
+        Z: 0.792743862
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.465398431
+      Pitch: 0.525398433
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.965398073
+      Yaw: 1.14039791
     Saved: ~
 Window Geometry:
   Camera view:

zed_wrapper/launch/zed_camera.launch

@@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
     <!-- Definition coordinate frames -->
     <arg name="publish_tf"                    default="true" />
+    <arg name="publish_map_tf"                default="true" />
     <arg name="pose_frame"                    default="map" />
     <arg name="odometry_frame"                default="odom" />
     <arg name="base_frame"                    default="zed_camera_center" />
@@ -62,6 +63,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
         <!-- publish pose frame -->
         <param name="publish_tf"            value="$(arg publish_tf)" />
+        <param name="publish_map_tf"        value="$(arg publish_map_tf)" />
+
 
         <!-- flip camera -->
         <param name="camera_flip"           value="$(arg camera_flip)" />

zed_wrapper/launch/zed_camera_nodelet.launch

@@ -38,6 +38,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
     <!-- Definition coordinate frames -->
     <arg name="publish_tf"                    default="true" />
+    <arg name="publish_map_tf"                default="true" />
     <arg name="pose_frame"                    default="map" />
     <arg name="odometry_frame"                default="odom" />
     <arg name="base_frame"                    default="zed_camera_center" />
@@ -64,6 +65,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
         <!-- publish pose frame -->
         <param name="publish_tf"            value="$(arg publish_tf)" />
+        <param name="publish_map_tf"        value="$(arg publish_map_tf)" />
 
         <!-- flip camera -->
         <param name="camera_flip"           value="$(arg camera_flip)" />

zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp

@@ -212,6 +212,10 @@ namespace zed_wrapper {
          */
         void set_pose(float xt, float yt, float zt, float rr, float pr, float yr);
 
+        /* \brief Utility to initialize the most used transforms
+         */
+        void initTransforms();
+
         /* \bried Start tracking loading the parameters from param server
          */
         void start_tracking();
@@ -353,6 +357,7 @@ namespace zed_wrapper {
         // TF Transforms
         tf2::Transform mOdom2MapTransf;
         tf2::Transform mBase2OdomTransf;
+        tf2::Transform mSensor2BaseTransf;
 
         // Zed object
         sl::InitParameters mZedParams;

zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp

@@ -248,14 +248,15 @@ namespace zed_wrapper {
         // SVO
         mNhNs.param<std::string>("svo_filepath", mSvoFilepath, std::string());
 
+        // Initialization transformation listener
+        mTfBuffer.reset(new tf2_ros::Buffer);
+        mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer));
+
         // Initialize tf2 transformation
         mNhNs.getParam("initial_tracking_pose", mInitialTrackPose);
         set_pose(mInitialTrackPose[0], mInitialTrackPose[1], mInitialTrackPose[2],
                  mInitialTrackPose[3], mInitialTrackPose[4], mInitialTrackPose[5]);
 
-        // Initialization transformation listener
-        mTfBuffer.reset(new tf2_ros::Buffer);
-        mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer));
 
         // Try to initialize the ZED
         if (!mSvoFilepath.empty()) {
@@ -638,6 +639,32 @@ namespace zed_wrapper {
         }
     }
 
+    void ZEDWrapperNodelet::initTransforms() {
+        // >>>>> Dynamic transforms
+        mBase2OdomTransf.setIdentity();
+        mOdom2MapTransf.setIdentity();
+        // <<<<< Dynamic transforms
+
+        // >>>>> Static transforms
+        // Sensor to Base link
+        try {
+            // Save the transformation from base to frame
+            geometry_msgs::TransformStamped s2b =
+                mTfBuffer->lookupTransform(mBaseFrameId, mDepthFrameId, mFrameTimestamp);
+            // Get the TF2 transformation
+            tf2::fromMsg(s2b.transform, mSensor2BaseTransf);
+        } catch (tf2::TransformException& ex) {
+            NODELET_WARN_THROTTLE(
+                10.0, "The tf from '%s' to '%s' does not seem to be available, "
+                "will assume it as identity!",
+                mDepthFrameId.c_str(), mBaseFrameId.c_str());
+            NODELET_DEBUG("Transform error: %s", ex.what());
+            mSensor2BaseTransf.setIdentity();
+        }
+        // <<<<< Static transforms
+
+    }
+
     void ZEDWrapperNodelet::set_pose(float xt, float yt, float zt, float rr,
                                      float pr, float yr) {
         // ROS pose
@@ -648,9 +675,7 @@ namespace zed_wrapper {
         //        mBase2OdomTransf.setRotation(q);
         //        mOdom2MapTransf.setIdentity();
 
-        mBase2OdomTransf.setIdentity();
-        mBase2OdomTransf.setIdentity();
-        mOdom2MapTransf.setIdentity();
+        initTransforms();
 
         // SL pose
         sl::float4 q_vec;
@@ -698,8 +723,8 @@ namespace zed_wrapper {
             NODELET_WARN_STREAM("Invalid Initial Pose size (" << mInitialTrackPose.size()
                                 << "). Using Identity");
             mInitialPoseSl.setIdentity();
-            mOdom2MapTransf.setIdentity();
-            mBase2OdomTransf.setIdentity();
+
+            initTransforms();
         } else {
             set_pose(mInitialTrackPose[0], mInitialTrackPose[1], mInitialTrackPose[2],
                      mInitialTrackPose[3], mInitialTrackPose[4], mInitialTrackPose[5]);
@@ -733,8 +758,8 @@ namespace zed_wrapper {
             NODELET_WARN_STREAM("Invalid Initial Pose size (" << mInitialTrackPose.size()
                                 << "). Using Identity");
             mInitialPoseSl.setIdentity();
-            mOdom2MapTransf.setIdentity();
-            mBase2OdomTransf.setIdentity();
+
+            initTransforms();
         } else {
             set_pose(mInitialTrackPose[0], mInitialTrackPose[1], mInitialTrackPose[2],
                      mInitialTrackPose[3], mInitialTrackPose[4], mInitialTrackPose[5]);
@@ -770,7 +795,7 @@ namespace zed_wrapper {
     void ZEDWrapperNodelet::publishOdom(tf2::Transform base2odomTransf, sl::Pose& slPose, ros::Time t) {
         nav_msgs::Odometry odom;
         odom.header.stamp = t;
-        odom.header.frame_id = mOdometryFrameId; // odom_frame
+        odom.header.frame_id = mOdometryFrameId;
         odom.child_frame_id = mBaseFrameId;      // camera_frame
         // conversion from Tranform to message
         geometry_msgs::Transform base2odom = tf2::toMsg(base2odomTransf);
@@ -1203,7 +1228,6 @@ namespace zed_wrapper {
                 "will assume it as identity!",
                 mBaseFrameId.c_str(), mOdometryFrameId.c_str());
             NODELET_DEBUG("Transform error: %s", ex.what());
-
         }
 
         if (mPublishMapTf) {
@@ -1223,12 +1247,11 @@ namespace zed_wrapper {
             }
         }
 
-
         geometry_msgs::PoseStamped odomPose;
         geometry_msgs::PoseStamped mapPose;
 
         odomPose.header.stamp = mFrameTimestamp;
-        odomPose.header.frame_id = mOdometryFrameId; // odom_frame
+        odomPose.header.frame_id = mOdometryFrameId;
         // conversion from Tranform to message
         geometry_msgs::Transform base2odom = tf2::toMsg(base_to_odom);
         // Add all value in Pose message
@@ -1737,24 +1760,7 @@ namespace zed_wrapper {
 
                 mCamDataMutex.unlock();
 
-                // Transform from base to sensor
-                tf2::Transform sensor_to_base_transf;
-
-                // Look up the transformation from base frame to camera link
-                try {
-                    // Save the transformation from base to frame
-                    geometry_msgs::TransformStamped s2b =
-                        mTfBuffer->lookupTransform(mBaseFrameId, mDepthFrameId, mFrameTimestamp);
-                    // Get the TF2 transformation
-                    tf2::fromMsg(s2b.transform, sensor_to_base_transf);
-                } catch (tf2::TransformException& ex) {
-                    NODELET_WARN_THROTTLE(
-                        10.0, "The tf from '%s' to '%s' does not seem to be available, "
-                        "will assume it as identity!",
-                        mDepthFrameId.c_str(), mBaseFrameId.c_str());
-                    NODELET_DEBUG("Transform error: %s", ex.what());
-                    sensor_to_base_transf.setIdentity();
-                }
+
 
                 // Publish the odometry if someone has subscribed to
                 if (poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || cloudSubnumber > 0 ||
@@ -1780,7 +1786,7 @@ namespace zed_wrapper {
                             tf2::fromMsg(deltaTransf, deltaOdomTf);
                             // delta odom from sensor to base frame
                             tf2::Transform deltaOdomTf_base =
-                                sensor_to_base_transf * deltaOdomTf * sensor_to_base_transf.inverse();
+                                mSensor2BaseTransf * deltaOdomTf * mSensor2BaseTransf.inverse();
 
                             // Propagate Odom transform in time
                             mBase2OdomTransf = mBase2OdomTransf * deltaOdomTf_base;
@@ -1826,7 +1832,7 @@ namespace zed_wrapper {
                         /*tf2::Transform base_to_map_transform =
                             sensor_to_base_transf * sens_to_map_transf * sensor_to_base_transf.inverse();*/
 
-                        tf2::Transform base_to_map_transform = (sensor_to_base_transf * sens_to_map_transf.inverse()).inverse();
+                        tf2::Transform base_to_map_transform = (mSensor2BaseTransf * sens_to_map_transf.inverse()).inverse();
 
                         bool initOdom = false;