Uniformized Landmark msg with LandmarkDetection already in ros2 branc…

…h. Fixed right image conversion bug when converting from SensorData msg. Fixed some SensorData related subscribers not initialized.
introlab · Nov 20, 2023 · d974c9a · d974c9a
1 parent 67de27b
commit d974c9a
Show file tree

Hide file tree

Showing 7 changed files with 42 additions and 20 deletions.
diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp
@@ -1105,11 +1105,10 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::SensorData & msg)
 
 	// Image data
 	cv::Mat left, right;
-	cv_bridge::CvImageConstPtr leftRawPtr, rightRawPtr;
 	if(!msg.left.data.empty())
 	{
 		boost::shared_ptr<void const> trackedObject;
-		leftRawPtr = cv_bridge::toCvShare(msg.left, trackedObject);
+		cv_bridge::CvImageConstPtr leftRawPtr = cv_bridge::toCvShare(msg.left, trackedObject);
 
 		if(!(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
@@ -1143,7 +1142,7 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::SensorData & msg)
 	if(!msg.right.data.empty())
 	{
 		boost::shared_ptr<void const> trackedObject;
-		rightRawPtr = cv_bridge::toCvShare(msg.right, trackedObject);
+		cv_bridge::CvImageConstPtr rightRawPtr = cv_bridge::toCvShare(msg.right, trackedObject);
 
 		if(!(rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
@@ -1169,7 +1168,7 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::SensorData & msg)
 			}
 			else
 			{
-				right = cv_bridge::cvtColor(leftRawPtr, "mono8")->image;
+				right = cv_bridge::cvtColor(rightRawPtr, "mono8")->image;
 			}
 		}
 	}

diff --git a/rtabmap_msgs/CMakeLists.txt b/rtabmap_msgs/CMakeLists.txt
@@ -26,7 +26,8 @@ add_message_files(
    SensorData.msg
    Link.msg
    OdomInfo.msg
-   Landmark.msg
+   LandmarkDetection.msg
+   LandmarkDetections.msg
    Point2f.msg
    Point3f.msg
    Goal.msg

diff --git a/rtabmap_msgs/msg/Landmark.msg b/rtabmap_msgs/msg/Landmark.msg
diff --git a/rtabmap_msgs/msg/LandmarkDetection.msg b/rtabmap_msgs/msg/LandmarkDetection.msg
@@ -0,0 +1,22 @@
+
+# header.stamp: the timestamp of the detection (e.g. image timestamp)
+# header.frame_id: the base frame of pose (e.g., camera optical frame)
+std_msgs/Header header
+
+# Landmark's frame id
+string landmark_frame_id
+
+# Landmark's unique ID: should be >0
+int32 id
+
+# Size in meters of the landmark/tag (optional, set 0 to not use it).
+float32 size
+
+# Pose of the landmark in header.frame_id frame.
+# If covariance is unknown, keep it as null matrix.
+# rtabmap_slam/rtabmap's landmark_angular_variance and
+# landmark_linear_variance parameters can be used
+# for convenience if covariance is null.
+geometry_msgs/PoseWithCovariance pose
+
+
diff --git a/rtabmap_msgs/msg/LandmarkDetections.msg b/rtabmap_msgs/msg/LandmarkDetections.msg
@@ -0,0 +1,8 @@
+
+# header.stamp: the timestamp of the detection (e.g. image timestamp)
+# header.frame_id: the base frame of pose (e.g., camera optical frame)
+std_msgs/Header header
+
+LandmarkDetection[] landmarks
+
+
diff --git a/rtabmap_msgs/msg/SensorData.msg b/rtabmap_msgs/msg/SensorData.msg
@@ -57,10 +57,10 @@ EnvSensor[] env_sensors
 sensor_msgs/Imu imu
 geometry_msgs/Transform imu_local_transform
 
-Landmark[] landmarks
+LandmarkDetection[] landmarks
 
 # Ground truth
 geometry_msgs/Pose ground_truth_pose
 
 # GPS
-GPS gps
+GPS gps
diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp
@@ -191,6 +191,11 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbdXOdomDataInfo),
 #endif
 
+        // SensorData
+        SYNC_INIT(sensorDataInfo),
+        SYNC_INIT(sensorDataOdom),
+        SYNC_INIT(sensorDataOdomInfo),
+
 #ifdef RTABMAP_SYNC_MULTI_RGBD
 		// 2 RGBD
 		SYNC_INIT(rgbd2),