fix axis issue

realsense_examples/config/demo_rgbd.rviz

@@ -13,7 +13,7 @@ Panels:
         - /PointCloud21
         - /Image2
       Splitter Ratio: 0.5013054609298706
-    Tree Height: 140
+    Tree Height: 348
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -215,7 +215,7 @@ Visualization Manager:
       Unreliable: false
       Value: true
     - Class: rviz_default_plugins/Image
-      Enabled: true
+      Enabled: false
       Max Value: 1
       Median window: 5
       Min Value: 0
@@ -224,7 +224,7 @@ Visualization Manager:
       Queue Size: 10
       Topic: /camera/infra2/image_rect_raw
       Unreliable: false
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/Image
       Enabled: true
       Max Value: 1
@@ -236,10 +236,21 @@ Visualization Manager:
       Topic: /camera/color/image_raw
       Unreliable: false
       Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 10
+      Topic: /camera/aligned_depth_to_color/image_raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: camera_color_optical_frame
+    Fixed Frame: camera_link
     Frame Rate: 30
   Name: root
   Tools:
@@ -277,26 +288,26 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3297962546348572
+      Pitch: -0.029796218499541283
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.2695062160491943
+      Yaw: 2.7595081329345703
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1025
+  Height: 1385
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000026d000003a3fc0200000020fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003f000000cc000000cc00fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb000000100044006900730070006c00610079007301000000000000015b0000015b00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000111000000ab0000002900fffffffb0000000a0049006d00610067006501000001c2000000aa0000002900fffffffb0000000a0049006d0061006700650100000272000000b20000002900fffffffb0000000a0049006d0061006700650000000111000000ae0000000000000000fb0000000a0049006d00610067006500000001410000011b0000000000000000fb0000000a0049006d00610067006500000001af0000024e0000000000000000fb0000000a0049006d0061006700650000000331000000cc0000000000000000fb0000000a0049006d0061006700650100000343000000ba0000000000000000fb0000000a0049006d00610067006501000001f0000000a80000000000000000fb0000000a0049006d006100670065010000029e000000ab0000000000000000fb0000000a0049006d006100670065010000034f000000ae0000000000000000fb0000000a0049006d0061006700650100000162000000a60000000000000000fb0000000a0049006d006100670065010000020e0000009f0000000000000000fb0000000a0049006d00610067006501000002b3000000a20000000000000000fb0000000a0049006d006100670065010000035b000000a20000000000000000fb0000000a0049006d0061006700650100000111000000b50000000000000000fb0000000a0049006d00610067006501000001cc000000af0000000000000000fb0000000a0049006d0061006700650100000281000000bd0000000000000000fb0000000a0049006d0061006700650100000344000000b90000000000000000fb0000000a0049006d0061006700650100000111000000c60000000000000000fb0000000a0049006d00610067006501000001dd000000b30000000000000000fb0000000a0049006d0061006700650100000296000000b10000000000000000fb0000000a0049006d006100670065010000034d000000b00000000000000000fb0000000a0049006d006100670065010000032a000000b80000002900ffffff000000010000010f000003a3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000003a3000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003b5000003a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1853
-  X: 67
+  Width: 2489
+  X: 71
   Y: 27

realsense_examples/launch/rs_t265_and_d400.launch.py

@@ -30,10 +30,10 @@
 def generate_launch_description():
     # config the serial number and base frame id of each camera
     t265_base_frame_id = LaunchConfiguration('base_frame_id', default='t265_link')
-    t265_serial_no = LaunchConfiguration('serial_no', default='845412111144')
+    t265_serial_no = LaunchConfiguration('serial_no', default='845412110563')
 
     rgbd_base_frame_id = LaunchConfiguration('base_frame_id', default='d435_link')
-    rgbd_serial_no = LaunchConfiguration('serial_no', default='819312071869')
+    rgbd_serial_no = LaunchConfiguration('serial_no', default='841612070383')
 
     rviz_config_dir = os.path.join(get_package_share_directory('realsense_examples'), 'config', 'rs_cartographer.rviz')
 
@@ -68,6 +68,6 @@ def generate_launch_description():
         parameters=[{'serial_no':rgbd_serial_no, 
                 'base_frame_id': rgbd_base_frame_id,
                 'enable_pointcloud':'true',
-                'dense_pointcloud' : 'false'}]
+                'dense_pointcloud' : 'true'}]
         )
     return launch.LaunchDescription([rviz_node, tf_node, t265_node, rgbd_node])

realsense_ros/src/rs_base.cpp

@@ -259,14 +259,15 @@ void RealSenseBase::calculateTFAndPublish(const rs2::stream_profile & stream_in,
   Q = quaternion_optical * Q * quaternion_optical.inverse();
 
   Float3 translation{ex.translation[0], ex.translation[1], ex.translation[2]};
+  Float3 zero_trans{0, 0, 0};
   auto type = stream_in.stream_type();
   auto index = stream_in.stream_index();
   auto type_index = std::pair<rs2_stream, int>(type, index);
   if (type == RS2_STREAM_POSE) {
     Q = Q.inverse();
     composeTFMsgAndPublish(transform_ts, translation, Q, OPTICAL_FRAME_ID.at(type_index), base_frame_id_);
   } else {
-    composeTFMsgAndPublish(transform_ts, translation, Q, base_frame_id_, OPTICAL_FRAME_ID.at(type_index));
+    composeTFMsgAndPublish(transform_ts, translation, quaternion_optical, base_frame_id_, OPTICAL_FRAME_ID.at(type_index));
   }
 }
 

realsense_ros/src/rs_d435.cpp

@@ -239,9 +239,9 @@ void RealSenseD435::publishSparsePointCloud(const rs2::points & points, const rs
 
     for (size_t index = 0; index < valid_indices.size(); index++) {
       auto pnt_idx = valid_indices[index];
-      *iter_x = vertex[pnt_idx].z;
-      *iter_y = -vertex[pnt_idx].x;
-      *iter_z = -vertex[pnt_idx].y;
+      *iter_x = vertex[pnt_idx].x;
+      *iter_y = vertex[pnt_idx].y;
+      *iter_z = vertex[pnt_idx].z;
 
       float i = color_point[pnt_idx].u;
       float j = color_point[pnt_idx].v;
@@ -290,10 +290,9 @@ void RealSenseD435::publishSparsePointCloud(const rs2::points & points, const rs
 
     for (size_t index = 0; index < valid_indices.size(); index++) {
       auto pnt_idx = valid_indices[index];
-      *iter_x = vertex[pnt_idx].z;
-      *iter_y = -vertex[pnt_idx].x;
-      *iter_z = -vertex[pnt_idx].y;
-
+      *iter_x = vertex[pnt_idx].x;
+      *iter_y = vertex[pnt_idx].y;
+      *iter_z = vertex[pnt_idx].z;
       float i = color_point[pnt_idx].u;
       float j = color_point[pnt_idx].v;
       int pixx = static_cast<int>(i * width);
@@ -344,9 +343,9 @@ void RealSenseD435::publishDensePointCloud(const rs2::points & points, const rs2
     uint8_t * color_data = (uint8_t*)color_frame.get_data();
 
     for (size_t pnt_idx = 0; pnt_idx < pc_msg->width*pc_msg->height; pnt_idx++) {
-        *iter_x = vertex[pnt_idx].z;
-        *iter_y = -vertex[pnt_idx].x;
-        *iter_z = -vertex[pnt_idx].y;
+        *iter_x = vertex[pnt_idx].x;
+        *iter_y = vertex[pnt_idx].y;
+        *iter_z = vertex[pnt_idx].z;
 
         *iter_r = color_data[pnt_idx*channel_num];
         *iter_g = color_data[pnt_idx*channel_num+1];
@@ -386,9 +385,9 @@ void RealSenseD435::publishDensePointCloud(const rs2::points & points, const rs2
     uint8_t * color_data = (uint8_t*)color_frame.get_data();
 
     for (size_t pnt_idx = 0; pnt_idx < pc_msg->width*pc_msg->height; pnt_idx++) {
-      *iter_x = vertex[pnt_idx].z;
-      *iter_y = -vertex[pnt_idx].x;
-      *iter_z = -vertex[pnt_idx].y;
+      *iter_x = vertex[pnt_idx].x;
+      *iter_y = vertex[pnt_idx].y;
+      *iter_z = vertex[pnt_idx].z;
 
       *iter_r = color_data[pnt_idx*channel_num];
       *iter_g = color_data[pnt_idx*channel_num+1];