Fixed IMU frame publishing

 * changed names of transforms variables to a more clear form

rviz/zedm.rviz

@@ -6,9 +6,10 @@ Panels:
       Expanded:
         - /Global Options1
         - /RobotModel1/Links1
+        - /TF1
+        - /TF1/Frames1
         - /DepthCloud1/Occlusion Compensation1
         - /Imu1
-        - /Odometry1/Shape1
         - /Odometry1/Covariance1/Position1
       Splitter Ratio: 0.5
     Tree Height: 451
@@ -95,7 +96,7 @@ Visualization Manager:
       Visual Enabled: true
     - Class: rviz/TF
       Enabled: true
-      Frame Timeout: 15
+      Frame Timeout: 5
       Frames:
         All Enabled: false
         imu_link:
@@ -114,9 +115,9 @@ Visualization Manager:
           Value: false
         zed_right_camera_optical_frame:
           Value: false
-      Marker Scale: 1
+      Marker Scale: 0.5
       Name: TF
-      Show Arrows: false
+      Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
@@ -244,12 +245,12 @@ Visualization Manager:
       Topic: /zed/imu/data
       Unreliable: false
       Value: true
-      fixed_frame_orientation: false
+      fixed_frame_orientation: true
     - Alpha: 1
       Axes Length: 1
       Axes Radius: 0.100000001
       Class: rviz/Pose
-      Color: 255; 25; 0
+      Color: 0; 255; 0
       Enabled: true
       Head Length: 0.100000001
       Head Radius: 0.0500000007
@@ -260,7 +261,7 @@ Visualization Manager:
       Topic: /zed/map
       Unreliable: false
       Value: true
-    - Angle Tolerance: 0.100000001
+    - Angle Tolerance: 0.00999999978
       Class: rviz/Odometry
       Covariance:
         Orientation:
@@ -278,20 +279,20 @@ Visualization Manager:
           Value: true
         Value: true
       Enabled: true
-      Keep: 0
+      Keep: 1
       Name: Odometry
-      Position Tolerance: 0.100000001
+      Position Tolerance: 0.00100000005
       Shape:
         Alpha: 1
         Axes Length: 0.100000001
         Axes Radius: 0.00999999978
         Color: 255; 25; 0
-        Head Length: 0.300000012
-        Head Radius: 0.100000001
-        Shaft Length: 1
-        Shaft Radius: 0.0500000007
-        Value: Axes
-      Topic: ""
+        Head Length: 0.100000001
+        Head Radius: 0.0500000007
+        Shaft Length: 0.300000012
+        Shaft Radius: 0.0199999996
+        Value: Arrow
+      Topic: /zed/odom
       Unreliable: false
       Value: true
   Enabled: true
@@ -319,25 +320,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 3.55838561
+      Distance: 3.8035841
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.0342643671
-        Y: 0.298864007
-        Z: 0.638629913
+        X: 0.731076837
+        Y: 1.32646406
+        Z: 0.494817436
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.119796388
+      Pitch: 0.109797455
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.39409924
+      Yaw: 2.76409745
     Saved: ~
 Window Geometry:
   Camera:

src/nodelet/include/zed_wrapper_nodelet.hpp

@@ -90,7 +90,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
          * \param odom_base_transform : Transformation representing the camera pose from base frame
          * \param t : the ros::Time to stamp the image
          */
-    void publishOdom(tf2::Transform odom_base_transform, ros::Time t);
+    void publishOdom(tf2::Transform base_to_odom_transform, ros::Time t);
 
     /* \brief Publish the pose of the camera as a transformation
          * \param base_transform : Transformation representing the camera pose from base frame
@@ -102,7 +102,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
          * \param base_transform : Transformation representing the camera pose from base frame
          * \param t : the ros::Time to stamp the image
          */
-    void publishOdomFrame(tf2::Transform odom_base_transform, ros::Time t);
+    void publishOdomFrame(tf2::Transform base_to_odom_transform, ros::Time t);
 
     /* \brief Publish the pose of the imu as a transformation
          * \param base_transform : Transformation representing the imu pose from camera frame
@@ -289,13 +289,12 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
     bool spatial_memory;
 
     //Tracking variables
-    sl::Pose zed_pose;
-    sl::Pose odom_pose;
-    std::vector<float> initial_track_pose;
-    tf2::Transform pose_odom_transform;
-    tf2::Transform odom_base_transform;
-    tf2::Transform imu_base_transform;
     sl::Transform initial_pose_sl;
+    std::vector<float> initial_track_pose;
+
+    tf2::Transform odom_to_map_transform;
+    tf2::Transform base_to_odom_transform;
+
 
     // zed object
     sl::InitParameters param;