merged master->ros2

rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h

@@ -55,7 +55,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_msgs/msg/point3f.hpp>
 #include <rtabmap_msgs/msg/map_data.hpp>
 #include <rtabmap_msgs/msg/map_graph.hpp>
-#include <rtabmap_msgs/msg/node_data.hpp>
+#include <rtabmap_msgs/msg/node.hpp>
 #include <rtabmap_msgs/msg/odom_info.hpp>
 #include <rtabmap_msgs/msg/info.hpp>
 #include <rtabmap_msgs/msg/rgbd_image.hpp>
@@ -162,11 +162,18 @@ void mapGraphToROS(
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_msgs::msg::MapGraph & msg);
 
-rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::msg::NodeData & msg);
-void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::NodeData & msg);
+rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg);
+void sensorDataToROS(const rtabmap::SensorData & signature, rtabmap_msgs::msg::SensorData & msg, const std::string & frameId = "base_link", bool copyRawData = false);
 
-rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::msg::NodeData & msg);
-void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::NodeData & msg);
+rtabmap::Signature nodeFromROS(const rtabmap_msgs::msg::Node & msg);
+void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
+
+// DEPRECATED
+rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::msg::Node & msg);
+void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
+
+rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::msg::Node & msg);
+void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
 
 std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, bool ignoreData = false);
@@ -175,6 +182,9 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_msgs::msg::OdomIn
 cv::Mat userDataFromROS(const rtabmap_msgs::msg::UserData & dataMsg);
 void userDataToROS(const cv::Mat & data, rtabmap_msgs::msg::UserData & dataMsg, bool compress);
 
+rtabmap::IMU imuFromROS(const sensor_msgs::msg::Imu & msg, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
+void imuToROS(const rtabmap::IMU & imu, sensor_msgs::msg::Imu & msg);
+
 rtabmap::Landmarks landmarksFromROS(
 		const std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > & tags,
 		const std::string & frameId,

rtabmap_conversions/src/MsgConversion.cpp

@@ -999,7 +999,7 @@ void mapDataFromROS(
 	//Data
 	for(unsigned int i=0; i<msg.nodes.size(); ++i)
 	{
-		signatures.insert(std::make_pair(msg.nodes[i].id, nodeDataFromROS(msg.nodes[i])));
+		signatures.insert(std::make_pair(msg.nodes[i].id, nodeFromROS(msg.nodes[i])));
 	}
 }
 void mapDataToROS(
@@ -1019,7 +1019,7 @@ void mapDataToROS(
 		iter!=signatures.end();
 		++iter)
 	{
-		nodeDataToROS(iter->second, msg.nodes[index++]);
+		nodeToROS(iter->second, msg.nodes[index++]);
 	}
 }
 
@@ -1073,13 +1073,346 @@ void mapGraphToROS(
 	transformToGeometryMsg(map_to_odom, msg.map_to_odom);
 }
 
-rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::msg::NodeData & msg)
+rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg)
+{
+	rtabmap::SensorData s(
+			cv::Mat(),
+			0,
+			timestampFromROS(msg.header.stamp),
+			compressedMatFromBytes(msg.user_data));
+
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	std::vector<rtabmap::CameraModel> models;
+	bool isStereo = !msg.right_camera_info.empty();
+	if(isStereo)
+	{
+		// stereo model
+		if(msg.left_camera_info.size() == msg.right_camera_info.size() &&
+		   msg.local_transform.size() == msg.right_camera_info.size())
+		{
+			for(unsigned int i=0; i<msg.right_camera_info.size(); ++i)
+			{
+				stereoModels.push_back(stereoCameraModelFromROS(
+					msg.left_camera_info[i],
+					msg.right_camera_info[i],
+					transformFromGeometryMsg(msg.local_transform[i])
+				));
+			}
+		}
+	}
+	else
+	{
+		// multi-cameras model
+		if(msg.left_camera_info.size() &&
+		   msg.local_transform.size() == msg.left_camera_info.size())
+		{
+			for(unsigned int i=0; i<msg.left_camera_info.size(); ++i)
+			{
+				models.push_back(cameraModelFromROS(
+					msg.left_camera_info[i],
+					transformFromGeometryMsg(msg.local_transform[i])));
+			}
+		}
+	}
+
+	// Image data
+	cv::Mat left, right;
+	if(!msg.left.data.empty())
+	{
+		std::shared_ptr<void const> 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 ||
+			 leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+			 leftRawPtr->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+			 leftRawPtr->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+			 leftRawPtr->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+			 leftRawPtr->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+			 leftRawPtr->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8)))
+		{
+			UERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended), received type is %s. Will return data without left/rgb raw image.",
+					leftRawPtr->encoding.c_str());
+		}
+		else
+		{
+			if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+				leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
+			{
+				left = leftRawPtr->image.clone();
+			}
+			else if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+			{
+				left = cv_bridge::cvtColor(leftRawPtr, "mono8")->image;
+			}
+			else
+			{
+				left = cv_bridge::cvtColor(leftRawPtr, "bgr8")->image;
+			}
+		}
+	}
+	if(!msg.right.data.empty())
+	{
+		std::shared_ptr<void const> 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 ||
+			 rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+			 rightRawPtr->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+			 rightRawPtr->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+			 rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+			 rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0))
+	{
+			UERROR("Input type must be image=mono8,mono16,rgb8,bgr8,32FC1,16UC1, received type is %s. Will return data without right/depth raw image.",
+					rightRawPtr->encoding.c_str());
+		}
+		else
+		{
+			if(rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+				rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				(!isStereo && 
+				   (rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0||
+					rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+					rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)))
+			{
+				right = rightRawPtr->image.clone();
+			}
+			else
+			{
+				right = cv_bridge::cvtColor(rightRawPtr, "mono8")->image;
+			}
+		}
+	}
+
+	if(isStereo)
+	{
+		s.setStereoImage(
+			compressedMatFromBytes(msg.left_compressed),
+			compressedMatFromBytes(msg.right_compressed),
+			stereoModels);
+		if(!left.empty() && !right.empty())
+		{
+			s.setStereoImage(left, right, stereoModels, false);
+		}
+	}
+	else
+	{
+		s.setRGBDImage(
+			compressedMatFromBytes(msg.left_compressed),
+			compressedMatFromBytes(msg.right_compressed),
+			models);
+		if(!left.empty() && !right.empty())
+		{
+			s.setRGBDImage(left, right, models, false);
+		}
+	}
+
+	// Laser scan data
+	if(!msg.laser_scan_compressed.empty())
+	{
+		s.setLaserScan(rtabmap::LaserScan(
+			compressedMatFromBytes(msg.laser_scan_compressed),
+			msg.laser_scan_max_pts,
+			msg.laser_scan_max_range,
+			(rtabmap::LaserScan::Format)msg.laser_scan_format,
+			transformFromGeometryMsg(msg.laser_scan_local_transform)));
+	}
+	if(!msg.laser_scan.data.empty())
+	{
+		pcl::PCLPointCloud2 cloud;
+		pcl_conversions::toPCL(msg.laser_scan, cloud);
+		s.setLaserScan(rtabmap::LaserScan(
+			rtabmap::util3d::laserScanFromPointCloud(cloud),
+			msg.laser_scan_max_pts,
+			msg.laser_scan_max_range,
+			transformFromGeometryMsg(msg.laser_scan_local_transform)),
+			false);
+		UASSERT((rtabmap::LaserScan::Format)msg.laser_scan_format == s.laserScanRaw().format());
+	}
+
+	//convert features
+	std::vector<cv::KeyPoint> keypoints;
+	std::vector<cv::Point3f> keypoints3D;
+	cv::Mat descriptors;
+	if(!msg.key_points.empty())
+	{
+		keypoints = rtabmap_conversions::keypointsFromROS(msg.key_points);
+	}
+	if(!msg.points.empty())
+	{
+		keypoints3D = rtabmap_conversions::points3fFromROS(msg.points);
+	}
+	if(!msg.descriptors.empty())
+	{
+		descriptors = rtabmap::uncompressData(msg.descriptors);
+	}
+	s.setFeatures(keypoints, keypoints3D, descriptors);
+
+	s.setGlobalDescriptors(rtabmap_conversions::globalDescriptorsFromROS(msg.global_descriptors));
+	s.setEnvSensors(rtabmap_conversions::envSensorsFromROS(msg.env_sensors));
+	s.setOccupancyGrid(
+			compressedMatFromBytes(msg.grid_ground),
+			compressedMatFromBytes(msg.grid_obstacles),
+			compressedMatFromBytes(msg.grid_empty_cells),
+			msg.grid_cell_size,
+			point3fFromROS(msg.grid_view_point));
+	s.setGPS(rtabmap::GPS(msg.gps.stamp, msg.gps.longitude, msg.gps.latitude, msg.gps.altitude, msg.gps.error, msg.gps.bearing));
+	s.setIMU(rtabmap_conversions::imuFromROS(msg.imu, transformFromGeometryMsg(msg.imu_local_transform)));
+	return s;
+}
+void sensorDataToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::SensorData & msg, const std::string & frameId, bool copyRawData)
+{
+	// add data
+	msg.header.stamp = timestampToROS(data.stamp());
+	msg.header.frame_id = frameId;
+	transformToPoseMsg(data.groundTruth(), msg.ground_truth_pose);
+	msg.gps.stamp = data.gps().stamp();
+	msg.gps.longitude = data.gps().longitude();
+	msg.gps.latitude = data.gps().latitude();
+	msg.gps.altitude = data.gps().altitude();
+	msg.gps.error = data.gps().error();
+	msg.gps.bearing = data.gps().bearing();
+
+	//Calibration
+	if(data.cameraModels().size())
+	{
+		msg.left_camera_info.resize(data.cameraModels().size());
+		msg.local_transform.resize(data.cameraModels().size());
+		for(unsigned int i=0; i<data.cameraModels().size(); ++i)
+		{
+			cameraModelToROS(data.cameraModels()[i], msg.left_camera_info[i]);
+			transformToGeometryMsg(data.cameraModels()[i].localTransform(), msg.local_transform[i]);
+		}
+	}
+	else if(data.stereoCameraModels().size())
+	{
+		msg.left_camera_info.resize(data.stereoCameraModels().size());
+		msg.right_camera_info.resize(data.stereoCameraModels().size());
+		msg.local_transform.resize(data.stereoCameraModels().size());
+		for(unsigned int i=0; i<data.stereoCameraModels().size(); ++i)
+		{
+			cameraModelToROS(data.stereoCameraModels()[i].left(), msg.left_camera_info[i]);
+			cameraModelToROS(data.stereoCameraModels()[i].right(), msg.right_camera_info[i]);
+			transformToGeometryMsg(data.stereoCameraModels()[i].left().localTransform(), msg.local_transform[i]);
+		}
+	}
+
+	// Images
+	if(copyRawData)
+	{
+		if(!data.imageRaw().empty())
+		{
+			cv_bridge::CvImage cvImg;
+			cvImg.image = data.imageRaw();
+			UASSERT(data.imageRaw().type()==CV_8UC1 || data.imageRaw().type()==CV_8UC3);
+			cvImg.encoding = data.imageRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:sensor_msgs::image_encodings::BGR8;
+			cvImg.toImageMsg(msg.left);
+		}
+		if(!data.depthOrRightRaw().empty())
+		{
+			cv_bridge::CvImage cvDepth;
+			cvDepth.image = data.depthOrRightRaw();
+			UASSERT(data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type()==CV_16UC1 || data.depthOrRightRaw().type()==CV_32FC1);
+			cvDepth.encoding = data.depthOrRightRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:data.depthOrRightRaw().type()==CV_16UC1?sensor_msgs::image_encodings::TYPE_16UC1:sensor_msgs::image_encodings::TYPE_32FC1;
+			cvDepth.toImageMsg(msg.right);
+		}
+	}
+	compressedMatToBytes(data.imageCompressed(), msg.left_compressed);
+	compressedMatToBytes(data.depthOrRightCompressed(), msg.right_compressed);
+
+	// Laser scan
+	if(copyRawData && !data.laserScanRaw().empty())
+	{
+		pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw());
+		pcl_conversions::moveFromPCL(*cloud, msg.laser_scan);
+		msg.laser_scan_max_pts = data.laserScanCompressed().maxPoints();
+		msg.laser_scan_max_range = data.laserScanCompressed().rangeMax();
+		msg.laser_scan_format = data.laserScanCompressed().format();
+		transformToGeometryMsg(data.laserScanCompressed().localTransform(), msg.laser_scan_local_transform);
+	}
+	if(!data.laserScanCompressed().empty())
+	{
+		compressedMatToBytes(data.laserScanCompressed().data(), msg.laser_scan_compressed);
+		msg.laser_scan_max_pts = data.laserScanCompressed().maxPoints();
+		msg.laser_scan_max_range = data.laserScanCompressed().rangeMax();
+		msg.laser_scan_format = data.laserScanCompressed().format();
+		transformToGeometryMsg(data.laserScanCompressed().localTransform(), msg.laser_scan_local_transform);
+	}
+	
+	// user data
+	if(!data.userDataCompressed().empty())
+	{
+		compressedMatToBytes(data.userDataCompressed(), msg.user_data);
+		
+	}
+	else if(copyRawData && !data.userDataRaw().empty())
+	{
+		compressedMatToBytes(rtabmap::compressData2(data.userDataRaw()), msg.user_data);
+	}
+
+	// oocupancy grid
+	if(!data.gridGroundCellsCompressed().empty())
+	{
+		compressedMatToBytes(data.gridGroundCellsCompressed(), msg.grid_ground);
+		
+	}
+	else if(copyRawData && !data.gridGroundCellsRaw().empty())
+	{
+		compressedMatToBytes(rtabmap::compressData2(data.gridGroundCellsRaw()), msg.grid_ground);
+	}
+	if(!data.gridObstacleCellsCompressed().empty())
+	{
+		compressedMatToBytes(data.gridObstacleCellsCompressed(), msg.grid_obstacles);
+		
+	}
+	else if(copyRawData && !data.gridObstacleCellsRaw().empty())
+	{
+		compressedMatToBytes(rtabmap::compressData2(data.gridObstacleCellsRaw()), msg.grid_obstacles);
+	}
+	if(!data.gridEmptyCellsCompressed().empty())
+	{
+		compressedMatToBytes(data.gridEmptyCellsCompressed(), msg.grid_empty_cells);
+		
+	}
+	else if(copyRawData && !data.gridEmptyCellsRaw().empty())
+	{
+		compressedMatToBytes(rtabmap::compressData2(data.gridEmptyCellsRaw()), msg.grid_empty_cells);
+	}
+
+	point3fToROS(data.gridViewPoint(), msg.grid_view_point);
+	msg.grid_cell_size = data.gridCellSize();
+
+	//convert features
+	if(!data.keypoints().empty())
+	{
+		rtabmap_conversions::keypointsToROS(data.keypoints(), msg.key_points);
+	}
+	if(!data.keypoints3D().empty())
+	{
+		rtabmap_conversions::points3fToROS(data.keypoints3D(), msg.points);
+	}
+	if(!data.descriptors().empty())
+	{
+		msg.descriptors = rtabmap::compressData(data.descriptors());
+	}
+	if(!data.globalDescriptors().empty())
+	{
+		rtabmap_conversions::globalDescriptorsToROS(data.globalDescriptors(), msg.global_descriptors);
+	}
+
+	rtabmap_conversions::globalDescriptorsToROS(data.globalDescriptors(), msg.global_descriptors);
+	rtabmap_conversions::envSensorsToROS(data.envSensors(), msg.env_sensors);
+	rtabmap_conversions::imuToROS(data.imu(), msg.imu);
+	transformToGeometryMsg(data.imu().localTransform(), msg.imu_local_transform);
+}
+
+rtabmap::Signature nodeFromROS(const rtabmap_msgs::msg::Node & msg)
 {
 	//Features stuff...
 	std::multimap<int, int> words;
 	std::vector<cv::KeyPoint> wordsKpts;
 	std::vector<cv::Point3f> words3D;
-
 	cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.word_descriptors);
 
 	if(msg.word_id_keys.size() != msg.word_id_values.size())
@@ -1100,6 +1433,15 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::msg::NodeData & msg)
 		wordsDescriptors = cv::Mat();
 	}
 
+	rtabmap::Signature s(
+		msg.id,
+		msg.map_id,
+		msg.weight,
+		msg.stamp,
+		msg.label,
+		transformFromPoseMsg(msg.pose),
+		transformFromPoseMsg(msg.data.ground_truth_pose));
+
 	if(msg.word_id_keys.size() == msg.word_id_values.size())
 	{
 		for(unsigned int i=0; i<msg.word_id_keys.size(); ++i)
@@ -1124,108 +1466,11 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::msg::NodeData & msg)
 		}
 	}
 
-	std::vector<rtabmap::StereoCameraModel> stereoModels;
-	std::vector<rtabmap::CameraModel> models;
-	if(msg.baseline.size())
-	{
-		// stereo model
-		if(msg.fx.size() == msg.baseline.size() &&
-		   msg.fy.size() == msg.baseline.size() &&
-		   msg.cx.size() == msg.baseline.size() &&
-		   msg.cy.size() == msg.baseline.size() &&
-		   msg.width.size() == msg.baseline.size() &&
-		   msg.height.size() == msg.baseline.size() &&
-		   msg.local_transform.size() == msg.baseline.size())
-		{
-			for(unsigned int i=0; i<msg.fx.size(); ++i)
-			{
-				stereoModels.push_back(rtabmap::StereoCameraModel(
-						msg.fx[i],
-						msg.fy[i],
-						msg.cx[i],
-						msg.cy[i],
-						msg.baseline[i],
-						transformFromGeometryMsg(msg.local_transform[i]),
-						cv::Size(msg.width[i], msg.height[i])));
-			}
-		}
-	}
-	else
-	{
-		// multi-cameras model
-		if(msg.fx.size() &&
-		   msg.fx.size() == msg.fy.size() &&
-		   msg.fx.size() == msg.cx.size() &&
-		   msg.fx.size() == msg.cy.size() &&
-		   msg.fx.size() == msg.local_transform.size())
-		{
-			for(unsigned int i=0; i<msg.fx.size(); ++i)
-			{
-				if(msg.fx[i] == 0)
-				{
-					models.push_back(rtabmap::CameraModel());
-				}
-				else
-				{
-					models.push_back(rtabmap::CameraModel(
-							msg.fx[i],
-							msg.fy[i],
-							msg.cx[i],
-							msg.cy[i],
-							transformFromGeometryMsg(msg.local_transform[i]),
-							0.0,
-							cv::Size(msg.width[i], msg.height[i])));
-				}
-			}
-		}
-	}
-
-	rtabmap::Signature s(
-			msg.id,
-			msg.map_id,
-			msg.weight,
-			msg.stamp,
-			msg.label,
-			transformFromPoseMsg(msg.pose),
-			transformFromPoseMsg(msg.ground_truth_pose),
-			stereoModels.size()?
-				rtabmap::SensorData(
-					rtabmap::LaserScan(compressedMatFromBytes(msg.laser_scan),
-							msg.laser_scan_max_pts,
-							msg.laser_scan_max_range,
-							(rtabmap::LaserScan::Format)msg.laser_scan_format,
-							transformFromGeometryMsg(msg.laser_scan_local_transform)),
-					compressedMatFromBytes(msg.image),
-					compressedMatFromBytes(msg.depth),
-					stereoModels,
-					msg.id,
-					msg.stamp,
-					compressedMatFromBytes(msg.user_data)):
-				rtabmap::SensorData(
-					rtabmap::LaserScan(compressedMatFromBytes(msg.laser_scan),
-							msg.laser_scan_max_pts,
-							msg.laser_scan_max_range,
-							(rtabmap::LaserScan::Format)msg.laser_scan_format,
-							transformFromGeometryMsg(msg.laser_scan_local_transform)),
-					compressedMatFromBytes(msg.image),
-					compressedMatFromBytes(msg.depth),
-					models,
-					msg.id,
-					msg.stamp,
-					compressedMatFromBytes(msg.user_data)));
 	s.setWords(words, wordsKpts, words3D, wordsDescriptors);
-	s.sensorData().setGlobalDescriptors(rtabmap_conversions::globalDescriptorsFromROS(msg.global_descriptors));
-	s.sensorData().setEnvSensors(rtabmap_conversions::envSensorsFromROS(msg.env_sensors));
-	s.sensorData().setOccupancyGrid(
-			compressedMatFromBytes(msg.grid_ground),
-			compressedMatFromBytes(msg.grid_obstacles),
-			compressedMatFromBytes(msg.grid_empty_cells),
-			msg.grid_cell_size,
-			point3fFromROS(msg.grid_view_point));
-	s.sensorData().setGPS(rtabmap::GPS(msg.gps.stamp, msg.gps.longitude, msg.gps.latitude, msg.gps.altitude, msg.gps.error, msg.gps.bearing));
+	s.sensorData() = sensorDataFromROS(msg.data);
 	return s;
 }
-void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::NodeData & msg)
+void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg)
 {
 	// add data
 	msg.id = signature.id();
@@ -1234,68 +1479,6 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node
 	msg.stamp = signature.getStamp();
 	msg.label = signature.getLabel();
 	transformToPoseMsg(signature.getPose(), msg.pose);
-	transformToPoseMsg(signature.getGroundTruthPose(), msg.ground_truth_pose);
-	msg.gps.stamp = signature.sensorData().gps().stamp();
-	msg.gps.longitude = signature.sensorData().gps().longitude();
-	msg.gps.latitude = signature.sensorData().gps().latitude();
-	msg.gps.altitude = signature.sensorData().gps().altitude();
-	msg.gps.error = signature.sensorData().gps().error();
-	msg.gps.bearing = signature.sensorData().gps().bearing();
-	compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image);
-	compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
-	compressedMatToBytes(signature.sensorData().laserScanCompressed().data(), msg.laser_scan);
-	compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.user_data);
-	compressedMatToBytes(signature.sensorData().gridGroundCellsCompressed(), msg.grid_ground);
-	compressedMatToBytes(signature.sensorData().gridObstacleCellsCompressed(), msg.grid_obstacles);
-	compressedMatToBytes(signature.sensorData().gridEmptyCellsCompressed(), msg.grid_empty_cells);
-	point3fToROS(signature.sensorData().gridViewPoint(), msg.grid_view_point);
-	msg.grid_cell_size = signature.sensorData().gridCellSize();
-	msg.laser_scan_max_pts = signature.sensorData().laserScanCompressed().maxPoints();
-	msg.laser_scan_max_range = signature.sensorData().laserScanCompressed().rangeMax();
-	msg.laser_scan_format = signature.sensorData().laserScanCompressed().format();
-	transformToGeometryMsg(signature.sensorData().laserScanCompressed().localTransform(), msg.laser_scan_local_transform);
-	if(signature.sensorData().cameraModels().size())
-	{
-		msg.fx.resize(signature.sensorData().cameraModels().size());
-		msg.fy.resize(signature.sensorData().cameraModels().size());
-		msg.cx.resize(signature.sensorData().cameraModels().size());
-		msg.cy.resize(signature.sensorData().cameraModels().size());
-		msg.width.resize(signature.sensorData().cameraModels().size());
-		msg.height.resize(signature.sensorData().cameraModels().size());
-		msg.local_transform.resize(signature.sensorData().cameraModels().size());
-		for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
-		{
-			msg.fx[i] = signature.sensorData().cameraModels()[i].fx();
-			msg.fy[i] = signature.sensorData().cameraModels()[i].fy();
-			msg.cx[i] = signature.sensorData().cameraModels()[i].cx();
-			msg.cy[i] = signature.sensorData().cameraModels()[i].cy();
-			msg.width[i] = signature.sensorData().cameraModels()[i].imageWidth();
-			msg.height[i] = signature.sensorData().cameraModels()[i].imageHeight();
-			transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.local_transform[i]);
-		}
-	}
-	else if(signature.sensorData().stereoCameraModels().size())
-	{
-		msg.fx.resize(signature.sensorData().stereoCameraModels().size());
-		msg.fy.resize(signature.sensorData().stereoCameraModels().size());
-		msg.cx.resize(signature.sensorData().stereoCameraModels().size());
-		msg.cy.resize(signature.sensorData().stereoCameraModels().size());
-		msg.width.resize(signature.sensorData().stereoCameraModels().size());
-		msg.height.resize(signature.sensorData().stereoCameraModels().size());
-		msg.baseline.resize(signature.sensorData().stereoCameraModels().size());
-		msg.local_transform.resize(signature.sensorData().stereoCameraModels().size());
-		for(unsigned int i=0; i<signature.sensorData().stereoCameraModels().size(); ++i)
-		{
-			msg.fx[i] = signature.sensorData().stereoCameraModels()[i].left().fx();
-			msg.fy[i] = signature.sensorData().stereoCameraModels()[i].left().fy();
-			msg.cx[i] = signature.sensorData().stereoCameraModels()[i].left().cx();
-			msg.cy[i] = signature.sensorData().stereoCameraModels()[i].left().cy();
-			msg.width[i] = signature.sensorData().stereoCameraModels()[i].left().imageWidth();
-			msg.height[i] = signature.sensorData().stereoCameraModels()[i].left().imageHeight();
-			msg.baseline[i] = signature.sensorData().stereoCameraModels()[i].baseline();
-			transformToGeometryMsg(signature.sensorData().stereoCameraModels()[i].left().localTransform(), msg.local_transform[i]);
-		}
-	}
 
 	//Features stuff...
 	if(!signature.getWordsKpts().empty() &&
@@ -1355,11 +1538,20 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node
 		}
 	}
 
-	rtabmap_conversions::globalDescriptorsToROS(signature.sensorData().globalDescriptors(), msg.global_descriptors);
-	rtabmap_conversions::envSensorsToROS(signature.sensorData().envSensors(), msg.env_sensors);
+	sensorDataToROS(signature.sensorData(), msg.data);
+	transformToPoseMsg(signature.getGroundTruthPose(), msg.data.ground_truth_pose);
 }
 
-rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::msg::NodeData & msg)
+rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::msg::Node & msg)
+{
+	return nodeFromROS(msg);
+}
+void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg)
+{
+	nodeToROS(signature, msg);
+}
+
+rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::msg::Node & msg)
 {
 	rtabmap::Signature s(
 			msg.id,
@@ -1368,10 +1560,10 @@ rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::msg::NodeData & msg)
 			msg.stamp,
 			msg.label,
 			transformFromPoseMsg(msg.pose),
-			transformFromPoseMsg(msg.ground_truth_pose));
+			transformFromPoseMsg(msg.data.ground_truth_pose));
 	return s;
 }
-void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::NodeData & msg)
+void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg)
 {
 	// add data
 	msg.id = signature.id();
@@ -1380,7 +1572,7 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node
 	msg.stamp = signature.getStamp();
 	msg.label = signature.getLabel();
 	transformToPoseMsg(signature.getPose(), msg.pose);
-	transformToPoseMsg(signature.getGroundTruthPose(), msg.ground_truth_pose);
+	transformToPoseMsg(signature.getGroundTruthPose(), msg.data.ground_truth_pose);
 }
 
 std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info)
@@ -1675,6 +1867,43 @@ void userDataToROS(const cv::Mat & data, rtabmap_msgs::msg::UserData & dataMsg,
 	}
 }
 
+rtabmap::IMU imuFromROS(const sensor_msgs::msg::Imu & msg, const rtabmap::Transform & localTransform)
+{
+	return rtabmap::IMU(
+		cv::Vec4d(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w),
+		cv::Mat(3,3,CV_64FC1,(void*)msg.orientation_covariance.data()).clone(),
+		cv::Vec3d(msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z),
+		cv::Mat(3,3,CV_64FC1,(void*)msg.angular_velocity_covariance.data()).clone(),
+		cv::Vec3d(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z),
+		cv::Mat(3,3,CV_64FC1,(void*)msg.linear_acceleration_covariance.data()).clone(),
+		localTransform);
+}
+void imuToROS(const rtabmap::IMU & imu, sensor_msgs::msg::Imu & msg)
+{
+	msg.orientation.x = imu.orientation()[0];
+	msg.orientation.y = imu.orientation()[1];
+	msg.orientation.z = imu.orientation()[2];
+	msg.orientation.w = imu.orientation()[3];
+	if(!imu.orientationCovariance().empty())
+	{
+		memcpy((void*)msg.orientation_covariance.data(), imu.orientationCovariance().data, 9*sizeof(double));
+	}
+	msg.angular_velocity.x = imu.angularVelocity()[0];
+	msg.angular_velocity.y = imu.angularVelocity()[1];
+	msg.angular_velocity.z = imu.angularVelocity()[2];
+	if(!imu.angularVelocityCovariance().empty())
+	{
+		memcpy((void*)msg.angular_velocity_covariance.data(), imu.angularVelocityCovariance().data, 9*sizeof(double));
+	}
+	msg.linear_acceleration.x = imu.linearAcceleration()[0];
+	msg.linear_acceleration.y = imu.linearAcceleration()[1];
+	msg.linear_acceleration.z = imu.linearAcceleration()[2];
+	if(!imu.linearAccelerationCovariance().empty())
+	{
+		memcpy((void*)msg.linear_acceleration_covariance.data(), imu.linearAccelerationCovariance().data, 9*sizeof(double));
+	}
+}
+
 rtabmap::Landmarks landmarksFromROS(
 		const std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > & tags,
 		const std::string & frameId,