Add rtabmap_msgs/SensorData (#1055)

* Add rtabmap_msgs/SensorData

* After testing fixes

* Updated output topic name

* added explicit --logconsole for nodelets

* fixed intermediate nodes not generated

* Refactored SyncDiagnostic usage to handle nodelet name

* SyncDiagnostic: added TimeStampStatus. Odom: added new status when data not received yet

* rtabmap: Fixed parameters not updated when using nodelet

rtabmap_msgs/CMakeLists.txt

@@ -22,9 +22,11 @@ add_message_files(
    ScanDescriptor.msg
    MapData.msg
    MapGraph.msg
-   NodeData.msg
+   Node.msg
+   SensorData.msg
    Link.msg
    OdomInfo.msg
+   Landmark.msg
    Point2f.msg
    Point3f.msg
    Goal.msg

rtabmap_msgs/msg/Landmark.msg

@@ -0,0 +1,13 @@
+#class rtabmap::Landmark
+#{
+#   int id_;
+#   float size_;
+#	Transform pose_;
+#	cv::Mat covariance_;
+#}
+
+Header header
+int32 id
+float32 size
+geometry_msgs/Transform pose
+float64[36] covariance

rtabmap_msgs/msg/MapData.msg

@@ -9,7 +9,7 @@ MapGraph graph
 ##################
 # Graph data
 ##################
-NodeData[] nodes
+Node[] nodes
 
 
 

rtabmap_msgs/msg/Node.msg

@@ -0,0 +1,23 @@
+#class rtabmap::Signature
+
+int32 id
+int32 map_id
+int32 weight
+float64 stamp
+string label
+
+# Pose from odometry not corrected
+geometry_msgs/Pose pose
+
+# std::multimap<wordId, index>
+# std::vector<cv::Keypoint>
+# std::vector<cv::Point3f>
+int32[] word_id_keys
+int32[] word_id_values
+KeyPoint[] word_kpts
+Point3f[] word_pts
+# compressed descriptors
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] word_descriptors
+
+SensorData data

rtabmap_msgs/msg/NodeData.msg

@@ -1,70 +0,0 @@
-
-int32 id
-int32 mapId
-int32 weight
-float64 stamp
-string label
-
-# Pose from odometry not corrected
-geometry_msgs/Pose pose
-
-# Ground truth (optional)
-geometry_msgs/Pose groundTruthPose
-
-# GPS (optional)
-GPS gps
-
-# compressed image in /camera_link frame
-# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
-uint8[] image
-
-# compressed depth image in /camera_link frame
-# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
-uint8[] depth
-
-# Camera models
-float32[] fx
-float32[] fy
-float32[] cx
-float32[] cy
-float32[] width
-float32[] height
-float32[] baseline
-# local transform (/base_link -> /camera_link)
-geometry_msgs/Transform[] localTransform
-
-# compressed 2D or 3D laser scan
-# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
-uint8[] laserScan
-int32 laserScanMaxPts
-float32 laserScanMaxRange
-int32 laserScanFormat
-# local transform (/base_link -> /base_laser)
-geometry_msgs/Transform laserScanLocalTransform
-
-# compressed user data
-# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
-uint8[] userData
-
-# compressed occupancy grid
-# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
-uint8[] grid_ground
-uint8[] grid_obstacles
-uint8[] grid_empty_cells
-float32 grid_cell_size
-Point3f grid_view_point
-
-# std::multimap<wordId, index>
-# std::vector<cv::Keypoint>
-# std::vector<cv::Point3f>
-int32[] wordIdKeys
-int32[] wordIdValues
-KeyPoint[] wordKpts
-Point3f[] wordPts
-# compressed descriptors
-# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
-uint8[] wordDescriptors
-
-GlobalDescriptor[] globalDescriptors
-
-EnvSensor[] env_sensors

rtabmap_msgs/msg/SensorData.msg

@@ -0,0 +1,66 @@
+#class rtabmap::SensorData
+
+Header header
+
+# For RGB-D, left corresponds to rgb camera, and right corresponds to depth camera.
+
+# Raw images
+sensor_msgs/Image left
+sensor_msgs/Image right
+
+# Compressed images
+# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
+uint8[] left_compressed
+uint8[] right_compressed
+
+# Camera info
+sensor_msgs/CameraInfo[] left_camera_info
+sensor_msgs/CameraInfo[] right_camera_info
+
+# Transform from base frame to camera frame
+geometry_msgs/Transform[] local_transform
+
+# raw 2d or 3D laser scan
+sensor_msgs/PointCloud2 laser_scan
+# compressed 2D or 3D laser scan
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] laser_scan_compressed
+int32 laser_scan_max_pts
+float32 laser_scan_max_range
+int32 laser_scan_format
+# local transform (base frame -> laser frame)
+geometry_msgs/Transform laser_scan_local_transform
+
+# compressed user data
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] user_data
+
+# compressed occupancy grid
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] grid_ground
+uint8[] grid_obstacles
+uint8[] grid_empty_cells
+float32 grid_cell_size
+Point3f grid_view_point
+
+# Local features
+KeyPoint[] key_points
+Point3f[] points
+# compressed descriptors
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] descriptors
+
+GlobalDescriptor[] global_descriptors
+
+EnvSensor[] env_sensors
+
+sensor_msgs/Imu imu
+geometry_msgs/Transform imu_local_transform
+
+Landmark[] landmarks
+
+# Ground truth
+geometry_msgs/Pose ground_truth_pose
+
+# GPS
+GPS gps

rtabmap_msgs/srv/GetNodeData.srv

@@ -6,4 +6,4 @@ bool grid
 bool user_data
 ---
 #response
-NodeData[] data
+Node[] data