i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 06:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add rtabmap_msgs/SensorData (#1055)

Browse Source 
* 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
This commit is contained in:
matlabbe
2023-11-19 13:16:35 -08:00
committed by GitHub
parent 3801006832
commit 67de27b1ee
30 changed files with 1318 additions and 350 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h
+15 -5
View File
@@ -55,7 +55,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_msgs/Point3f.h>
#include <rtabmap_msgs/MapData.h>
#include <rtabmap_msgs/MapGraph.h>
#include <rtabmap_msgs/NodeData.h>
#include <rtabmap_msgs/Node.h>
#include <rtabmap_msgs/OdomInfo.h>
#include <rtabmap_msgs/Info.h>
#include <rtabmap_msgs/RGBDImage.h>
@@ -162,11 +162,18 @@ void mapGraphToROS(
const rtabmap::Transform & mapToOdom,
rtabmap_msgs::MapGraph & msg);
rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::NodeData & msg);
void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::NodeData & msg);
rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::SensorData & msg);
void sensorDataToROS(const rtabmap::SensorData & signature, rtabmap_msgs::SensorData & msg, const std::string & frameId = "base_link", bool copyRawData = false);
rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::NodeData & msg);
void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::NodeData & msg);
rtabmap::Signature nodeFromROS(const rtabmap_msgs::Node & msg);
void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::Node & msg);
// DEPRECATED
rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::Node & msg);
void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::Node & msg);
rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::Node & msg);
void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::Node & msg);
std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::OdomInfo & msg, bool ignoreData = false);
@@ -175,6 +182,9 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_msgs::OdomInfo &
cv::Mat userDataFromROS(const rtabmap_msgs::UserData & dataMsg);
void userDataToROS(const cv::Mat & data, rtabmap_msgs::UserData & dataMsg, bool compress);
rtabmap::IMU imuFromROS(const sensor_msgs::Imu & msg, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
void imuToROS(const rtabmap::IMU & imu, sensor_msgs::Imu & msg);
rtabmap::Landmarks landmarksFromROS(
const std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > & tags,
const std::string & frameId,
rtabmap_conversions/src/MsgConversion.cpp
+441 -210
View File
@@ -987,7 +987,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(
@@ -1007,7 +1007,7 @@ void mapDataToROS(
iter!=signatures.end();
++iter)
{
nodeDataToROS(iter->second, msg.nodes[index++]);
nodeToROS(iter->second, msg.nodes[index++]);
}
}
@@ -1061,229 +1061,414 @@ void mapGraphToROS(
transformToGeometryMsg(mapToOdom, msg.mapToOdom);
}
rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::NodeData & msg)
rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::SensorData & msg)
{
//Features stuff...
std::multimap<int, int> words;
std::vector<cv::KeyPoint> wordsKpts;
std::vector<cv::Point3f> words3D;
cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.wordDescriptors);
if(msg.wordIdKeys.size() != msg.wordIdValues.size())
{
ROS_ERROR("Word ID keys and values should be the same size (%d, %d)!", (int)msg.wordIdKeys.size(), (int)msg.wordIdValues.size());
}
if(!msg.wordKpts.empty() && msg.wordKpts.size() != msg.wordIdKeys.size())
{
ROS_ERROR("Word IDs and 2D keypoints should be the same size (%d, %d)!", (int)msg.wordIdKeys.size(), (int)msg.wordKpts.size());
}
if(!msg.wordPts.empty() && msg.wordPts.size() != msg.wordIdKeys.size())
{
ROS_ERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.wordIdKeys.size(), (int)msg.wordPts.size());
}
if(!wordsDescriptors.empty() && wordsDescriptors.rows != (int)msg.wordIdKeys.size())
{
ROS_ERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.wordIdKeys.size(), wordsDescriptors.rows);
wordsDescriptors = cv::Mat();
}
if(msg.wordIdKeys.size() == msg.wordIdValues.size())
{
for(unsigned int i=0; i<msg.wordIdKeys.size(); ++i)
{
words.insert(std::make_pair(msg.wordIdKeys.at(i), msg.wordIdValues.at(i))); // ID to index
if(msg.wordIdKeys.size() == msg.wordKpts.size())
{
if(wordsKpts.empty())
{
wordsKpts.reserve(msg.wordKpts.size());
}
wordsKpts.push_back(keypointFromROS(msg.wordKpts.at(i)));
}
if(msg.wordIdKeys.size() == msg.wordPts.size())
{
if(words3D.empty())
{
words3D.reserve(msg.wordPts.size());
}
words3D.push_back(point3fFromROS(msg.wordPts[i]));
}
}
}
rtabmap::SensorData s(
cv::Mat(),
msg.header.seq,
msg.header.stamp.toSec(),
compressedMatFromBytes(msg.user_data));
std::vector<rtabmap::StereoCameraModel> stereoModels;
std::vector<rtabmap::CameraModel> models;
if(msg.baseline.size())
bool isStereo = !msg.right_camera_info.empty();
if(isStereo)
{
// 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.localTransform.size() == msg.baseline.size())
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.fx.size(); ++i)
for(unsigned int i=0; i<msg.right_camera_info.size(); ++i)
{
stereoModels.push_back(rtabmap::StereoCameraModel(
msg.fx[i],
msg.fy[i],
msg.cx[i],
msg.cy[i],
msg.baseline[i],
transformFromGeometryMsg(msg.localTransform[i]),
cv::Size(msg.width[i], msg.height[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.fx.size() &&
msg.fx.size() == msg.fy.size() &&
msg.fx.size() == msg.cx.size() &&
msg.fx.size() == msg.cy.size() &&
msg.fx.size() == msg.localTransform.size())
if(msg.left_camera_info.size() &&
msg.local_transform.size() == msg.left_camera_info.size())
{
for(unsigned int i=0; i<msg.fx.size(); ++i)
for(unsigned int i=0; i<msg.left_camera_info.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.localTransform[i]),
0.0,
cv::Size(msg.width[i], msg.height[i])));
}
models.push_back(cameraModelFromROS(
msg.left_camera_info[i],
transformFromGeometryMsg(msg.local_transform[i])));
}
}
}
rtabmap::Signature s(
msg.id,
msg.mapId,
msg.weight,
msg.stamp,
msg.label,
transformFromPoseMsg(msg.pose),
transformFromPoseMsg(msg.groundTruthPose),
stereoModels.size()?
rtabmap::SensorData(
rtabmap::LaserScan(compressedMatFromBytes(msg.laserScan),
msg.laserScanMaxPts,
msg.laserScanMaxRange,
(rtabmap::LaserScan::Format)msg.laserScanFormat,
transformFromGeometryMsg(msg.laserScanLocalTransform)),
compressedMatFromBytes(msg.image),
compressedMatFromBytes(msg.depth),
stereoModels,
msg.id,
msg.stamp,
compressedMatFromBytes(msg.userData)):
rtabmap::SensorData(
rtabmap::LaserScan(compressedMatFromBytes(msg.laserScan),
msg.laserScanMaxPts,
msg.laserScanMaxRange,
(rtabmap::LaserScan::Format)msg.laserScanFormat,
transformFromGeometryMsg(msg.laserScanLocalTransform)),
compressedMatFromBytes(msg.image),
compressedMatFromBytes(msg.depth),
models,
msg.id,
msg.stamp,
compressedMatFromBytes(msg.userData)));
s.setWords(words, wordsKpts, words3D, wordsDescriptors);
s.sensorData().setGlobalDescriptors(rtabmap_conversions::globalDescriptorsFromROS(msg.globalDescriptors));
s.sensorData().setEnvSensors(rtabmap_conversions::envSensorsFromROS(msg.env_sensors));
s.sensorData().setOccupancyGrid(
// 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);
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)))
{
ROS_ERROR("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())
{
boost::shared_ptr<void const> trackedObject;
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))
{
ROS_ERROR("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(leftRawPtr, "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.sensorData().setGPS(rtabmap::GPS(msg.gps.stamp, msg.gps.longitude, msg.gps.latitude, msg.gps.altitude, msg.gps.error, msg.gps.bearing));
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 nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::NodeData & msg)
void sensorDataToROS(const rtabmap::SensorData & data, rtabmap_msgs::SensorData & msg, const std::string & frameId, bool copyRawData)
{
// add data
msg.header.seq = data.id();
msg.header.stamp = ros::Time(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::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())
{
ROS_ERROR("Word ID keys and values should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), (int)msg.word_id_values.size());
}
if(!msg.word_kpts.empty() && msg.word_kpts.size() != msg.word_id_keys.size())
{
ROS_ERROR("Word IDs and 2D keypoints should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), (int)msg.word_kpts.size());
}
if(!msg.word_pts.empty() && msg.word_pts.size() != msg.word_id_keys.size())
{
ROS_ERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), (int)msg.word_pts.size());
}
if(!wordsDescriptors.empty() && wordsDescriptors.rows != (int)msg.word_id_keys.size())
{
ROS_ERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), wordsDescriptors.rows);
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)
{
words.insert(std::make_pair(msg.word_id_keys.at(i), msg.word_id_values.at(i))); // ID to index
if(msg.word_id_keys.size() == msg.word_kpts.size())
{
if(wordsKpts.empty())
{
wordsKpts.reserve(msg.word_kpts.size());
}
wordsKpts.push_back(keypointFromROS(msg.word_kpts.at(i)));
}
if(msg.word_id_keys.size() == msg.word_pts.size())
{
if(words3D.empty())
{
words3D.reserve(msg.word_pts.size());
}
words3D.push_back(point3fFromROS(msg.word_pts[i]));
}
}
}
s.setWords(words, wordsKpts, words3D, wordsDescriptors);
s.sensorData() = sensorDataFromROS(msg.data);
return s;
}
void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::Node & msg)
{
// add data
msg.id = signature.id();
msg.mapId = signature.mapId();
msg.map_id = signature.mapId();
msg.weight = signature.getWeight();
msg.stamp = signature.getStamp();
msg.label = signature.getLabel();
transformToPoseMsg(signature.getPose(), msg.pose);
transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose);
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.laserScan);
compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.userData);
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.laserScanMaxPts = signature.sensorData().laserScanCompressed().maxPoints();
msg.laserScanMaxRange = signature.sensorData().laserScanCompressed().rangeMax();
msg.laserScanFormat = signature.sensorData().laserScanCompressed().format();
transformToGeometryMsg(signature.sensorData().laserScanCompressed().localTransform(), msg.laserScanLocalTransform);
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.localTransform.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.localTransform[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.localTransform.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.localTransform[i]);
}
}
//Features stuff...
if(!signature.getWordsKpts().empty() &&
signature.getWords().size() != signature.getWordsKpts().size())
@@ -1301,29 +1486,29 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::NodeData
(int)signature.getWords3().size());
}
int i=0;
msg.wordIdKeys.resize(signature.getWords().size());
msg.wordIdValues.resize(signature.getWords().size());
msg.word_id_keys.resize(signature.getWords().size());
msg.word_id_values.resize(signature.getWords().size());
for(std::multimap<int, int>::const_iterator iter=signature.getWords().begin();
iter!=signature.getWords().end();
++iter)
{
msg.wordIdKeys.at(i) = iter->first;
msg.wordIdValues.at(i) = iter->second;
msg.word_id_keys.at(i) = iter->first;
msg.word_id_values.at(i) = iter->second;
if(signature.getWordsKpts().size() == signature.getWords().size())
{
if(msg.wordKpts.empty())
if(msg.word_kpts.empty())
{
msg.wordKpts.resize(signature.getWords().size());
msg.word_kpts.resize(signature.getWords().size());
}
keypointToROS(signature.getWordsKpts().at(i), msg.wordKpts.at(i));
keypointToROS(signature.getWordsKpts().at(i), msg.word_kpts.at(i));
}
if(signature.getWords3().size() == signature.getWords().size())
{
if(msg.wordPts.empty())
if(msg.word_pts.empty())
{
msg.wordPts.resize(signature.getWords().size());
msg.word_pts.resize(signature.getWords().size());
}
point3fToROS(signature.getWords3().at(i), msg.wordPts.at(i));
point3fToROS(signature.getWords3().at(i), msg.word_pts.at(i));
}
++i;
}
@@ -1332,7 +1517,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::NodeData
{
if(signature.getWordsDescriptors().rows == (int)signature.getWords().size())
{
msg.wordDescriptors = rtabmap::compressData(signature.getWordsDescriptors());
msg.word_descriptors = rtabmap::compressData(signature.getWordsDescriptors());
}
else
{
@@ -1342,32 +1527,41 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::NodeData
}
}
rtabmap_conversions::globalDescriptorsToROS(signature.sensorData().globalDescriptors(), msg.globalDescriptors);
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::NodeData & msg)
rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::Node & msg)
{
return nodeFromROS(msg);
}
void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::Node & msg)
{
nodeToROS(signature, msg);
}
rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::Node & msg)
{
rtabmap::Signature s(
msg.id,
msg.mapId,
msg.map_id,
msg.weight,
msg.stamp,
msg.label,
transformFromPoseMsg(msg.pose),
transformFromPoseMsg(msg.groundTruthPose));
transformFromPoseMsg(msg.data.ground_truth_pose));
return s;
}
void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::NodeData & msg)
void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::Node & msg)
{
// add data
msg.id = signature.id();
msg.mapId = signature.mapId();
msg.map_id = signature.mapId();
msg.weight = signature.getWeight();
msg.stamp = signature.getStamp();
msg.label = signature.getLabel();
transformToPoseMsg(signature.getPose(), msg.pose);
transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose);
transformToPoseMsg(signature.getGroundTruthPose(), msg.data.ground_truth_pose);
}
std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info)
@@ -1662,6 +1856,43 @@ void userDataToROS(const cv::Mat & data, rtabmap_msgs::UserData & dataMsg, bool
}
}
rtabmap::IMU imuFromROS(const sensor_msgs::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::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::PoseWithCovarianceStamped, float> > & tags,
const std::string & frameId,
rtabmap_examples/launch/test_sensor_data.launch
+191
View File
@@ -0,0 +1,191 @@
<?xml version="1.0"?>
<!--
Examples with OAK-D camera.
$ roslaunch depthai_examples stereo_inertial_node.launch depth_aligned:=false
$ rosrun imu_filter_madgwick imu_filter_node \
imu/data_raw:=/stereo_inertial_publisher/imu \
imu/data:=/stereo_inertial_publisher/imu/data \
_use_mag:=false \
_publish_tf:=false
Stereo:
$ roslaunch rtabmap_examples test_sensor_data.launch \
stereo:=true \
left_topic:=/stereo_inertial_publisher/left/image_rect \
right_topic:=/stereo_inertial_publisher/right/image_rect \
left_info_topic:=/stereo_inertial_publisher/left/camera_info \
right_info_topic:=/stereo_inertial_publisher/right/camera_info \
imu_topic:=/stereo_inertial_publisher/imu/data \
frame_id:=oak-d_frame \
approx_sync:=true \
approx_sync_max_interval:=0.001 \
wait_imu_to_init:=true
RGB-D:
$ roslaunch rtabmap_examples test_sensor_data.launch \
rgb_topic:=/stereo_inertial_publisher/right/image_rect \
depth_topic:=/stereo_inertial_publisher/stereo/depth \
camera_info_topic:=/stereo_inertial_publisher/right/camera_info \
imu_topic:=/stereo_inertial_publisher/imu/data \
frame_id:=oak-d_frame \
approx_sync:=true \
approx_sync_max_interval:=0.001 \
wait_imu_to_init:=true
-->
<launch>
<arg name="features_only" default="false"/>
<arg name="compression_format" default=".jpg"/> <!-- ".jpg" or ".png" -->
<arg name="parallel_compression" default="true"/>
<arg name="use_nodelets" default="true"/>
<arg name="stereo" default="false"/>
<arg name="frame_id" default="camera_link"/>
<arg name="rtabmap_args" default="--delete_db_on_start --logconsole"/> <!-- delete_db_on_start -->
<arg name="odom_args" default="$(arg rtabmap_args)"/>
<!-- RGB-D related topics -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<!-- Stereo related topics -->
<arg name="left_topic" default="/camera/left/image_rect" />
<arg name="right_topic" default="/camera/right/image_rect" />
<arg name="left_info_topic" default="/camera/left/camera_info" />
<arg name="right_info_topic" default="/camera/right/camera_info" />
<arg name="imu_topic" default="/imu/data" />
<arg name="wait_imu_to_init" default="false" />
<arg name="approx_sync" default="$(eval not stereo)" />
<arg name="approx_sync_max_interval" default="0" />
<group ns="rtabmap">
<!-- WITH NODELETS -->
<group if="$(arg use_nodelets)">
<node pkg="nodelet" type="nodelet" name="manager" args="manager" output="screen"/>
<!-- Use Stereo synchronization -->
<node if="$(arg stereo)" pkg="nodelet" type="nodelet" name="stereo_sync" args="load rtabmap_sync/stereo_sync manager">
<remap from="left/image_rect" to="$(arg left_topic)"/>
<remap from="right/image_rect" to="$(arg right_topic)"/>
<remap from="left/camera_info" to="$(arg left_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_info_topic)"/>
<param name="approx_sync" value="$(arg approx_sync)"/>
<param name="approx_sync_max_interval" value="$(arg approx_sync_max_interval)"/>
</node>
<!-- Use RGBD synchronization -->
<node unless="$(arg stereo)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_sync/rgbd_sync manager">
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="approx_sync" value="$(arg approx_sync)"/>
<param name="approx_sync_max_interval" value="$(arg approx_sync_max_interval)"/>
</node>
<!-- Stereo Odometry -->
<node if="$(arg stereo)" pkg="nodelet" type="nodelet" name="stereo_odometry" args="load rtabmap_odom/stereo_odometry manager $(arg odom_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="keep_color" type="bool" value="true"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
</node>
<!-- RGB-D Odometry -->
<node unless="$(arg stereo)" pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_odom/rgbd_odometry manager $(arg odom_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="keep_color" type="bool" value="true"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
</node>
<!-- RTAB-Map -->
<node pkg="nodelet" type="nodelet" name="rtabmap" args="load rtabmap_slam/rtabmap manager $(arg rtabmap_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_sensor_data" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap if="$(arg features_only)" from="sensor_data" to="odom_sensor_data/features"/>
<remap unless="$(arg features_only)" from="sensor_data" to="odom_sensor_data/raw"/>
</node>
</group>
<!-- WITHOUT NODELETS -->
<group unless="$(arg use_nodelets)">
<!-- Use Stereo synchronization -->
<node if="$(arg stereo)" pkg="rtabmap_sync" type="stereo_sync" name="stereo_sync" output="screen">
<remap from="left/image_rect" to="$(arg left_topic)"/>
<remap from="right/image_rect" to="$(arg right_topic)"/>
<remap from="left/camera_info" to="$(arg left_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_info_topic)"/>
<param name="approx_sync" value="$(arg approx_sync)"/>
<param name="approx_sync_max_interval" value="$(arg approx_sync_max_interval)"/>
</node>
<!-- Use RGBD synchronization -->
<node unless="$(arg stereo)" pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="approx_sync" value="$(arg approx_sync)"/>
<param name="approx_sync_max_interval" value="$(arg approx_sync_max_interval)"/>
</node>
<!-- Stereo Odometry -->
<node if="$(arg stereo)" pkg="rtabmap_odom" type="stereo_odometry" name="stereo_odometry" args="$(arg odom_args)" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="keep_color" type="bool" value="true"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
</node>
<!-- RGB-D Odometry -->
<node unless="$(arg stereo)" pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" args="$(arg odom_args)" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="keep_color" type="bool" value="true"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
</node>
<!-- RTAB-Map -->
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_sensor_data" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap if="$(arg features_only)" from="sensor_data" to="odom_sensor_data/features"/>
<remap unless="$(arg features_only)" from="sensor_data" to="odom_sensor_data/raw"/>
</node>
</group>
<!-- Visualization -->
<node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_sensor_data" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap if="$(arg features_only)" from="sensor_data" to="odom_sensor_data/features"/>
<remap unless="$(arg features_only)" from="sensor_data" to="odom_sensor_data/raw"/>
</node>
</group>
</launch>
rtabmap_examples/src/ExternalLoopDetectionExample.cpp
+1 -1
View File
@@ -201,7 +201,7 @@ int main(int argc, char** argv)
int addedNodes = 0;
for(size_t i=0; i<mapRes.data.nodes.size(); ++i)
{
rtabmap::Signature s = rtabmap_conversions::nodeDataFromROS(mapRes.data.nodes.at(i));
rtabmap::Signature s = rtabmap_conversions::nodeFromROS(mapRes.data.nodes.at(i));
rtabmap::SensorData compressedData = s.sensorData();
s.sensorData().uncompressData();
if(loopClosureDetector.process(s.sensorData(), rtabmap::Transform()))
rtabmap_msgs/CMakeLists.txt
+3 -1
View File
@@ -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
+13
View File
@@ -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
+1 -1
View File
@@ -9,7 +9,7 @@ MapGraph graph
##################
# Graph data
##################
NodeData[] nodes
Node[] nodes
rtabmap_msgs/msg/Node.msg
+23
View File
@@ -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
-70
View File
@@ -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
+66
View File
@@ -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
+1 -1
View File
@@ -6,4 +6,4 @@ bool grid
bool user_data
---
#response
NodeData[] data
Node[] data
rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+9 -1
View File
@@ -55,7 +55,7 @@ class Odometry;
namespace rtabmap_odom {
class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic
class OdometryROS : public nodelet::Nodelet
{
public:
@@ -113,6 +113,7 @@ private:
bool waitForTransform_;
double waitForTransformDuration_;
bool publishNullWhenLost_;
bool publishCompressedSensorData_;
rtabmap::ParametersMap parameters_;
ros::Publisher odomPub_;
@@ -122,6 +123,9 @@ private:
ros::Publisher odomLocalScanMap_;
ros::Publisher odomLastFrame_;
ros::Publisher odomRgbdImagePub_;
ros::Publisher odomSensorDataPub_;
ros::Publisher odomSensorDataFeaturesPub_;
ros::Publisher odomSensorDataCompressedPub_;
ros::ServiceServer resetSrv_;
ros::ServiceServer resetToPoseSrv_;
ros::ServiceServer pauseSrv_;
@@ -146,6 +150,8 @@ private:
double expectedUpdateRate_;
double maxUpdateRate_;
double minUpdateRate_;
std::string compressionImgFormat_;
bool compressionParallelized_;
int odomStrategy_;
bool waitIMUToinit_;
bool imuProcessed_;
@@ -162,8 +168,10 @@ private:
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
private:
bool lost_;
bool dataReceived_;
};
OdomStatusTask statusDiagnostic_;
std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
};
}
rtabmap_odom/src/OdometryROS.cpp
+115 -8
View File
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/Compression.h>
#include "rtabmap_conversions/MsgConversion.h"
#include "rtabmap_msgs/OdomInfo.h"
#include "rtabmap/utilite/UConversion.h"
@@ -57,7 +58,6 @@ using namespace rtabmap;
namespace rtabmap_odom {
OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
rtabmap_sync::SyncDiagnostic(0.5),
odometry_(0),
frameId_("base_link"),
odomFrameId_("odom"),
@@ -71,6 +71,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
publishNullWhenLost_(true),
publishCompressedSensorData_(false),
paused_(false),
resetCountdown_(0),
resetCurrentCount_(0),
@@ -81,6 +82,8 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
expectedUpdateRate_(0.0),
maxUpdateRate_(0.0),
minUpdateRate_(0.0),
compressionImgFormat_(".jpg"),
compressionParallelized_(true),
odomStrategy_(Parameters::defaultOdomStrategy()),
waitIMUToinit_(false),
imuProcessed_(false)
@@ -105,6 +108,9 @@ void OdometryROS::onInit()
odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
odomRgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("odom_rgbd_image", 1);
odomSensorDataPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/raw", 1);
odomSensorDataFeaturesPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/features", 1);
odomSensorDataCompressedPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/compressed", 1);
Transform initialPose = Transform::getIdentity();
std::string initialPoseStr;
@@ -143,6 +149,9 @@ void OdometryROS::onInit()
pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_);
pnh.param("min_update_rate", minUpdateRate_, minUpdateRate_);
pnh.param("sensor_data_compression_format", compressionImgFormat_, compressionImgFormat_);
pnh.param("sensor_data_parallel_compression", compressionParallelized_, compressionParallelized_);
pnh.param("wait_imu_to_init", waitIMUToinit_, waitIMUToinit_);
int eventLevel = ULogger::kFatal;
@@ -168,6 +177,7 @@ void OdometryROS::onInit()
NODELET_INFO("Odometry: ground_truth_base_frame_id = %s", groundTruthBaseFrameId_.c_str());
NODELET_INFO("Odometry: config_path = %s", configPath.c_str());
NODELET_INFO("Odometry: publish_null_when_lost = %s", publishNullWhenLost_?"true":"false");
NODELET_INFO("Odometry: publish_compressed_sensor_data = %s", publishCompressedSensorData_?"true":"false");
NODELET_INFO("Odometry: guess_frame_id = %s", guessFrameId_.c_str());
NODELET_INFO("Odometry: guess_min_translation = %f", guessMinTranslation_);
NODELET_INFO("Odometry: guess_min_rotation = %f", guessMinRotation_);
@@ -176,6 +186,8 @@ void OdometryROS::onInit()
NODELET_INFO("Odometry: max_update_rate = %f Hz", maxUpdateRate_);
NODELET_INFO("Odometry: min_update_rate = %f Hz", minUpdateRate_);
NODELET_INFO("Odometry: wait_imu_to_init = %s", waitIMUToinit_?"true":"false");
NODELET_INFO("Odometry: sensor_data_compression_format = %s", compressionImgFormat_.c_str());
NODELET_INFO("Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false");
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
if(configPath.size() && configPath.at(0) != '/')
@@ -373,9 +385,10 @@ void OdometryROS::onInit()
void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic)
{
NODELET_INFO("%s", subscribedTopicsMsg.c_str());
syncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(getNodeHandle(), getPrivateNodeHandle(), getName(), 0.5));
std::vector<diagnostic_updater::DiagnosticTask*> tasks;
tasks.push_back(&statusDiagnostic_);
initDiagnostic(subscribedTopic,
syncDiagnostic_->init(subscribedTopic,
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
@@ -902,6 +915,8 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
}
postProcessData(data, header);
if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers())
{
if(!header.frame_id.empty())
@@ -917,10 +932,96 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
}
postProcessData(data, header);
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
if(odomSensorDataPub_.getNumSubscribers() || odomSensorDataFeaturesPub_.getNumSubscribers())
{
rtabmap_msgs::SensorData msg;
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_.getNumSubscribers());
msg.header.stamp = header.stamp; // use corresponding time stamp to image
if(odomSensorDataPub_.getNumSubscribers())
{
odomSensorDataPub_.publish(msg);
}
if(odomSensorDataFeaturesPub_.getNumSubscribers())
{
// remove data
msg.left = sensor_msgs::Image();
msg.right = sensor_msgs::Image();
msg.laser_scan = sensor_msgs::PointCloud2();
msg.grid_ground.clear();
msg.grid_obstacles.clear();
msg.grid_empty_cells.clear();
odomSensorDataFeaturesPub_.publish(msg);
}
}
if(odomSensorDataCompressedPub_.getNumSubscribers())
{
cv::Mat compressedImage;
cv::Mat compressedDepth;
cv::Mat compressedScan;
if(compressionParallelized_)
{
rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_);
rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data());
if(!data.imageRaw().empty())
{
ctImage.start();
}
if(!data.depthOrRightRaw().empty())
{
ctDepth.start();
}
if(!data.laserScanRaw().isEmpty())
{
ctLaserScan.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
compressedImage = ctImage.getCompressedData();
compressedDepth = ctDepth.getCompressedData();
compressedScan = ctLaserScan.getCompressedData();
}
else
{
compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_);
compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
compressedScan = compressData2(data.laserScanRaw().data());
}
if(!compressedImage.empty() && !data.stereoCameraModels().empty())
{
data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false);
}
else if(!compressedImage.empty() && !data.cameraModels().empty())
{
data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false);
}
if(!compressedScan.empty())
{
data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f?
LaserScan(compressedScan,
data.laserScanRaw().maxPoints(),
data.laserScanRaw().rangeMax(),
data.laserScanRaw().format(),
data.laserScanRaw().localTransform()):
LaserScan(compressedScan,
data.laserScanRaw().format(),
data.laserScanRaw().rangeMin(),
data.laserScanRaw().rangeMax(),
data.laserScanRaw().angleMin(),
data.laserScanRaw().angleMax(),
data.laserScanRaw().angleIncrement(),
data.laserScanRaw().localTransform()), false);
}
rtabmap_msgs::SensorData msg;
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false);
msg.header.stamp = header.stamp; // use corresponding time stamp to image
odomSensorDataCompressedPub_.publish(msg);
}
if(visParams_)
{
if(icpParams_)
@@ -938,10 +1039,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
statusDiagnostic_.setStatus(pose.isNull());
if(!pose.isNull())
if(syncDiagnostic_.get() && !pose.isNull())
{
double curentRate = 1.0/(ros::WallTime::now()-time).toSec();
tick(header.stamp,
syncDiagnostic_->tick(header.stamp,
maxUpdateRate_>0 && maxUpdateRate_ < curentRate ? maxUpdateRate_:
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate);
@@ -1034,17 +1135,23 @@ bool OdometryROS::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Respon
OdometryROS::OdomStatusTask::OdomStatusTask() :
diagnostic_updater::DiagnosticTask("Odom status"),
lost_(false)
lost_(false),
dataReceived_(false)
{}
void OdometryROS::OdomStatusTask::setStatus(bool isLost)
{
dataReceived_ = true;
lost_ = isLost;
}
void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if(lost_)
if(!dataReceived_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No data received!");
}
else if(lost_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Lost!");
}
rtabmap_rviz_plugins/src/MapCloudDisplay.cpp
+1 -1
View File
@@ -289,7 +289,7 @@ void MapCloudDisplay::processMapData(const rtabmap_msgs::MapData& map)
int id = map.nodes[i].id;
// Always refresh the cloud if there are data
rtabmap::Signature s = rtabmap_conversions::nodeDataFromROS(map.nodes[i]);
rtabmap::Signature s = rtabmap_conversions::nodeFromROS(map.nodes[i]);
if((fromDepth &&
!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
+4
View File
@@ -151,6 +151,10 @@ private:
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::UserDataConstPtr & userDataMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg);
virtual void commonSensorDataCallback(
const rtabmap_msgs::SensorDataConstPtr & sensorDataMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg);
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
rtabmap_slam/src/CoreWrapper.cpp
+52 -8
View File
@@ -776,7 +776,8 @@ void CoreWrapper::onInit()
!this->isSubscribedToStereo() &&
!this->isSubscribedToRGBD() &&
!this->isSubscribedToRGB() &&
(this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || this->isSubscribedToOdom()))
(this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || this->isSubscribedToOdom()) &&
!this->isSubscribedToSensorData())
{
NODELET_WARN("There is no image subscription, bag-of-words loop closure detection will be disabled...");
int kpMaxFeatures = Parameters::defaultKpMaxFeatures();
@@ -1754,6 +1755,51 @@ void CoreWrapper::commonOdomCallback(
covariance_ = cv::Mat();
}
void CoreWrapper::commonSensorDataCallback(
const rtabmap_msgs::SensorDataConstPtr & sensorDataMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
{
UTimer timerConversion;
UASSERT(sensorDataMsg.get());
std::string odomFrameId = odomFrameId_;
if(odomMsg.get())
{
odomFrameId = odomMsg->header.frame_id;
if(!odomUpdate(odomMsg, sensorDataMsg->header.stamp))
{
return;
}
}
else if(!odomTFUpdate(sensorDataMsg->header.stamp))
{
return;
}
SensorData data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg);
if(lastPoseIntermediate_)
{
data.setId(-1);
}
OdometryInfo odomInfo;
if(odomInfoMsg.get())
{
odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg);
}
process(lastPoseStamp_,
data,
lastPose_,
lastPoseVelocity_,
odomFrameId,
covariance_,
odomInfo,
timerConversion.ticks());
covariance_ = cv::Mat();
}
void CoreWrapper::process(
const ros::Time & stamp,
SensorData & data,
@@ -2684,7 +2730,7 @@ void CoreWrapper::goalNodeCallback(const rtabmap_msgs::GoalConstPtr & msg)
bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ros::NodeHandle pnh("~");
ros::NodeHandle & pnh = getPrivateNodeHandle();
for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
std::string vStr;
@@ -2789,8 +2835,7 @@ bool CoreWrapper::pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
{
paused_ = true;
NODELET_INFO("rtabmap: paused!");
ros::NodeHandle pnh("~");
pnh.setParam("is_rtabmap_paused", true);
getPrivateNodeHandle().setParam("is_rtabmap_paused", true);
}
return true;
}
@@ -2805,8 +2850,7 @@ bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp
{
paused_ = false;
NODELET_INFO("rtabmap: resumed!");
ros::NodeHandle pnh("~");
pnh.setParam("is_rtabmap_paused", false);
getPrivateNodeHandle().setParam("is_rtabmap_paused", false);
}
return true;
}
@@ -3265,8 +3309,8 @@ bool CoreWrapper::getNodeDataCallback(rtabmap_msgs::GetNodeData::Request& req, r
if(s.id()>0)
{
rtabmap_msgs::NodeData msg;
rtabmap_conversions::nodeDataToROS(s, msg);
rtabmap_msgs::Node msg;
rtabmap_conversions::nodeToROS(s, msg);
res.data.push_back(msg);
}
}
rtabmap_sync/CMakeLists.txt
+1
View File
@@ -55,6 +55,7 @@ SET(rtabmap_sync_lib_src
src/impl/CommonDataSubscriberRGBDX.cpp
src/impl/CommonDataSubscriberScan.cpp
src/impl/CommonDataSubscriberOdom.cpp
src/impl/CommonDataSubscriberSensorData.cpp
)
IF(RTABMAP_SYNC_MULTI_RGBD)
SET(rtabmap_sync_lib_src
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
+32 -2
View File
@@ -50,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_msgs/UserData.h>
#include <rtabmap_msgs/OdomInfo.h>
#include <rtabmap_msgs/ScanDescriptor.h>
#include <rtabmap_msgs/SensorData.h>
#include <rtabmap_sync/CommonDataSubscriberDefines.h>
#include <rtabmap_sync/SyncDiagnostic.h>
@@ -57,7 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync {
class CommonDataSubscriber : public SyncDiagnostic {
class CommonDataSubscriber {
public:
CommonDataSubscriber(bool gui);
virtual ~CommonDataSubscriber();
@@ -69,8 +70,9 @@ public:
bool isSubscribedToRGBD() const {return subscribedToRGBD_;}
bool isSubscribedToScan2d() const {return subscribedToScan2d_;}
bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
bool isSubscribedToSensorData() const {return subscribedToSensorData_;}
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom();}
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();}
int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
int getQueueSize() const {return queueSize_;}
bool isApproxSync() const {return approxSync_;}
@@ -107,6 +109,10 @@ protected:
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::UserDataConstPtr & userDataMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg) = 0;
virtual void commonSensorDataCallback(
const rtabmap_msgs::SensorDataConstPtr & sensorDataMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg) = 0;
void commonSingleCameraCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -123,6 +129,8 @@ protected:
const std::vector<rtabmap_msgs::Point3f> & localPoints3d = std::vector<rtabmap_msgs::Point3f>(),
const cv::Mat & localDescriptors = cv::Mat());
void tick(const ros::Time & stamp, double targetFrequency = 0);
private:
void setupDepthCallbacks(
ros::NodeHandle & nh,
@@ -232,6 +240,13 @@ private:
int queueSize,
bool approxSync);
#endif
void setupSensorDataCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
void setupScanCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
@@ -261,6 +276,7 @@ private:
bool subscribedToRGB_;
bool subscribedToOdom_;
bool subscribedToRGBD_;
bool subscribedToSensorData_;
bool subscribedToScan2d_;
bool subscribedToScan3d_;
bool subscribedToScanDescriptor_;
@@ -278,6 +294,10 @@ private:
ros::Subscriber rgbdXSubOnly_;
message_filters::Subscriber<rtabmap_msgs::RGBDImages> rgbdXSub_;
//for sensor data callback
ros::Subscriber sensorDataSubOnly_;
message_filters::Subscriber<rtabmap_msgs::SensorData> sensorDataSub_;
//stereo callback
image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_;
@@ -296,6 +316,8 @@ private:
ros::Subscriber scanDescSubOnly_;
ros::Subscriber odomSubOnly_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
// RGB + Depth
DATA_SYNCS3(depth, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
DATA_SYNCS4(depthScan2d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
@@ -448,6 +470,14 @@ private:
DATA_SYNCS4(rgbdXOdomDataInfo, nav_msgs::Odometry, rtabmap_msgs::UserData, rtabmap_msgs::RGBDImages, rtabmap_msgs::OdomInfo);
#endif
// SensorData
void sensorDataCallback(const rtabmap_msgs::SensorDataConstPtr&);
DATA_SYNCS2(sensorDataInfo, rtabmap_msgs::SensorData, rtabmap_msgs::OdomInfo);
// SensorData + Odom
DATA_SYNCS2(sensorDataOdom, nav_msgs::Odometry, rtabmap_msgs::SensorData);
DATA_SYNCS3(sensorDataOdomInfo, nav_msgs::Odometry, rtabmap_msgs::SensorData, rtabmap_msgs::OdomInfo);
#ifdef RTABMAP_SYNC_MULTI_RGBD
// 2 RGBD
DATA_SYNCS2(rgbd2, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h
+11 -4
View File
@@ -12,8 +12,11 @@ namespace rtabmap_sync {
class SyncDiagnostic {
public:
SyncDiagnostic(double tolerance = 0.1, int windowSize = 5) :
SyncDiagnostic(ros::NodeHandle h = ros::NodeHandle(), ros::NodeHandle ph = ros::NodeHandle("~"), std::string nodeName = ros::this_node::getName(), double tolerance = 0.1, int windowSize = 5) :
diagnosticUpdater_(h, ph, nodeName),
frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)),
timeStampStatus_(diagnostic_updater::TimeStampStatusParam()),
compositeTask_("Sync status"),
lastCallbackCalledStamp_(ros::Time::now().toSec()-1),
targetFrequency_(0.0),
windowSize_(windowSize)
@@ -21,8 +24,7 @@ class SyncDiagnostic {
UASSERT(windowSize_ >= 1);
}
protected:
void initDiagnostic(
void init(
const std::string & topic,
const std::string & topicsNotReceivedWarningMsg,
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>())
@@ -35,7 +37,9 @@ protected:
// Assuming format is /back_camera/left/image, we want "back_camera"
strList.pop_back();
}
diagnosticUpdater_.add(frequencyStatus_);
compositeTask_.addTask(&frequencyStatus_);
compositeTask_.addTask(&timeStampStatus_);
diagnosticUpdater_.add(compositeTask_);
for(size_t i=0; i<otherTasks.size(); ++i)
{
diagnosticUpdater_.add(*otherTasks[i]);
@@ -48,6 +52,7 @@ protected:
void tick(const ros::Time & stamp, double targetFrequency = 0)
{
frequencyStatus_.tick();
timeStampStatus_.tick(stamp);
double singlePeriod = stamp.toSec() - lastCallbackCalledStamp_;
window_.push_back(singlePeriod);
@@ -91,6 +96,8 @@ private:
std::string topicsNotReceivedWarningMsg_;
diagnostic_updater::Updater diagnosticUpdater_;
diagnostic_updater::FrequencyStatus frequencyStatus_;
diagnostic_updater::TimeStampStatus timeStampStatus_;
diagnostic_updater::CompositeDiagnosticTask compositeTask_;
ros::Timer diagnosticTimer_;
double lastCallbackCalledStamp_;
double targetFrequency_;
rtabmap_sync/src/CommonDataSubscriber.cpp
+80 -14
View File
@@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync {
CommonDataSubscriber::CommonDataSubscriber(bool gui) :
SyncDiagnostic(0.5),
queueSize_(10),
approxSync_(true),
subscribedToDepth_(!gui),
@@ -38,6 +37,7 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
subscribedToRGB_(!gui),
subscribedToOdom_(false),
subscribedToRGBD_(false),
subscribedToSensorData_(false),
subscribedToScan2d_(false),
subscribedToScan3d_(false),
subscribedToScanDescriptor_(false),
@@ -380,6 +380,7 @@ void CommonDataSubscriber::setupCallbacks(
pnh.param("subscribe_scan_descriptor", subscribeScanDesc, subscribeScanDesc);
pnh.param("subscribe_stereo", subscribedToStereo_, subscribedToStereo_);
pnh.param("subscribe_rgbd", subscribedToRGBD_, subscribedToRGBD_);
pnh.param("subscribe_sensor_data", subscribedToSensorData_, subscribedToSensorData_);
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
pnh.param("subscribe_odom", subscribeOdom, subscribeOdom);
@@ -403,21 +404,51 @@ void CommonDataSubscriber::setupCallbacks(
ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgb cannot be true at the same time. Parameter subscribe_rgb is set to false.");
subscribedToRGB_ = false;
}
if(subscribedToDepth_ && subscribedToRGBD_)
if(subscribedToRGBD_)
{
ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
subscribedToDepth_ = false;
subscribedToRGB_ = false;
if(subscribedToDepth_)
{
ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
subscribedToDepth_ = false;
subscribedToRGB_ = false;
}
if(subscribedToRGB_)
{
ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgb is set to false.");
subscribedToRGB_ = false;
}
if(subscribedToStereo_)
{
ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
subscribedToStereo_ = false;
}
}
if(subscribedToRGB_ && subscribedToRGBD_)
if(subscribedToSensorData_)
{
ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgb is set to false.");
subscribedToRGB_ = false;
}
if(subscribedToStereo_ && subscribedToRGBD_)
{
ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
subscribedToStereo_ = false;
if(!subscribedToRGBD_)
{
if(subscribedToDepth_)
{
ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_sensor_data cannot be true at the same time. Parameter subscribe_depth is set to false.");
subscribedToDepth_ = false;
subscribedToRGB_ = false;
}
if(subscribedToRGB_)
{
ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_sensor_data cannot be true at the same time. Parameter subscribe_rgb is set to false.");
subscribedToRGB_ = false;
}
if(subscribedToStereo_)
{
ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_sensor_data cannot be true at the same time. Parameter subscribe_stereo is set to false.");
subscribedToStereo_ = false;
}
}
else
{
ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgbd is set to false.");
subscribedToRGBD_ = false;
}
}
if(subscribeScan2d && subscribeScan3d)
{
@@ -434,6 +465,21 @@ void CommonDataSubscriber::setupCallbacks(
ROS_WARN("rtabmap: Parameters subscribe_scan_cloud and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
subscribeScan3d = false;
}
if(subscribedToSensorData_ && subscribeScan2d)
{
ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_scan cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
subscribeScan2d = false;
}
if(subscribedToSensorData_ && subscribeScan3d)
{
ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
subscribeScan3d = false;
}
if(subscribedToSensorData_ && subscribeScanDesc)
{
ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_descriptor is set to false.");
subscribeScanDesc = false;
}
if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
{
if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_ && !subscribedToRGB_)
@@ -470,6 +516,7 @@ void CommonDataSubscriber::setupCallbacks(
ROS_INFO("%s: subscribe_rgb = %s", name.c_str(), subscribedToRGB_?"true":"false");
ROS_INFO("%s: subscribe_stereo = %s", name.c_str(), subscribedToStereo_?"true":"false");
ROS_INFO("%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras);
ROS_INFO("%s: subscribe_sensor_data = %s", name.c_str(), subscribedToSensorData_?"true":"false");
ROS_INFO("%s: subscribe_odom_info = %s", name.c_str(), subscribeOdomInfo?"true":"false");
ROS_INFO("%s: subscribe_user_data = %s", name.c_str(), subscribeUserData?"true":"false");
ROS_INFO("%s: subscribe_scan = %s", name.c_str(), subscribeScan2d?"true":"false");
@@ -648,6 +695,16 @@ void CommonDataSubscriber::setupCallbacks(
queueSize_,
approxSync_);
}
else if(subscribedToSensorData_)
{
setupSensorDataCallbacks(
nh,
pnh,
subscribedToOdom_,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
else if(subscribedToOdom_)
{
setupOdomCallbacks(
@@ -662,7 +719,8 @@ void CommonDataSubscriber::setupCallbacks(
if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
{
ROS_INFO("%s", subscribedTopicsMsg_.c_str());
initDiagnostic("",
syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, name, 0.5));
syncDiagnostic_->init("",
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. If topics are coming from different computers, make sure "
@@ -1046,4 +1104,12 @@ void CommonDataSubscriber::commonSingleCameraCallback(
localDescriptorsMsgs);
}
void CommonDataSubscriber::tick(const ros::Time & stamp, double targetFrequency)
{
if(syncDiagnostic_.get())
{
syncDiagnostic_->tick(stamp, targetFrequency);
}
}
} /* namespace rtabmap_sync */
rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp
+1
View File
@@ -523,6 +523,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
}
else
{
rgbdXSub_.unsubscribe();
rgbdXSubOnly_ = nh.subscribe("rgbd_images", queueSize, &CommonDataSubscriber::rgbdXCallback, this);
subscribedTopicsMsg_ =
rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp
+113
View File
@@ -0,0 +1,113 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_sync/CommonDataSubscriber.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_sync {
// SensorData
void CommonDataSubscriber::sensorDataCallback(
const rtabmap_msgs::SensorDataConstPtr& imagesMsg)
{
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
}
void CommonDataSubscriber::sensorDataInfoCallback(
const rtabmap_msgs::SensorDataConstPtr& imagesMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
{
nav_msgs::OdometryConstPtr odomMsg; // Null
commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
}
// SensorData + Odom
void CommonDataSubscriber::sensorDataOdomCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::SensorDataConstPtr& imagesMsg)
{
rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
}
void CommonDataSubscriber::sensorDataOdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::SensorDataConstPtr& imagesMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
{
commonSensorDataCallback(imagesMsg, odomMsg, odomInfoMsg);
}
void CommonDataSubscriber::setupSensorDataCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
{
ROS_INFO("Setup SensorData callback");
sensorDataSub_.subscribe(nh, "sensor_data", queueSize);
if(subscribeOdom)
{
odomSub_.subscribe(nh, "odom", queueSize);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync, queueSize, odomSub_, sensorDataSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync, queueSize, odomSub_, sensorDataSub_);
}
}
else
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync, queueSize, sensorDataSub_, odomInfoSub_);
}
else
{
sensorDataSub_.unsubscribe();
sensorDataSubOnly_ = nh.subscribe("sensor_data", queueSize, &CommonDataSubscriber::sensorDataCallback, this);
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(),
sensorDataSubOnly_.getTopic().c_str());
}
}
}
} /* namespace rtabmap_sync */
rtabmap_sync/src/nodelets/rgb_sync.cpp
+6 -3
View File
@@ -56,7 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class RgbSync : public nodelet::Nodelet, public SyncDiagnostic
class RgbSync : public nodelet::Nodelet
{
public:
RgbSync() :
@@ -123,7 +123,8 @@ private:
cameraInfoSub_.getTopic().c_str());
NODELET_INFO(subscribedTopicsMsg.c_str());
initDiagnostic(rgb_nh.resolveName("image_rect"),
syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
syncDiagnostic_->init(rgb_nh.resolveName("image_rect"),
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
@@ -137,7 +138,7 @@ private:
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
tick(image->header.stamp);
syncDiagnostic_->tick(image->header.stamp);
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
@@ -205,6 +206,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_sync::RgbSync, nodelet::Nodelet);
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+6 -3
View File
@@ -58,7 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class RGBDSync : public nodelet::Nodelet, public SyncDiagnostic
class RGBDSync : public nodelet::Nodelet
{
public:
RGBDSync() :
@@ -142,7 +142,8 @@ private:
cameraInfoSub_.getTopic().c_str());
NODELET_INFO(subscribedTopicsMsg.c_str());
initDiagnostic(rgb_nh.resolveName("image"),
syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
syncDiagnostic_->init(rgb_nh.resolveName("image"),
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
@@ -157,7 +158,7 @@ private:
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
tick(image->header.stamp);
syncDiagnostic_->tick(image->header.stamp);
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
@@ -304,6 +305,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_sync::RGBDSync, nodelet::Nodelet);
rtabmap_sync/src/nodelets/rgbdx_sync.cpp
+13 -9
View File
@@ -41,7 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class RGBDXSync : public nodelet::Nodelet, public SyncDiagnostic
class RGBDXSync : public nodelet::Nodelet
{
public:
RGBDXSync() :
@@ -167,7 +167,8 @@ private:
NODELET_INFO(subscribedTopicsMsg.c_str());
// Setup diagnostic
initDiagnostic("",
syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
syncDiagnostic_->init("",
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
@@ -189,13 +190,16 @@ private:
ros::Publisher rgbdImagesPub_;
std::vector<message_filters::Subscriber<rtabmap_msgs::RGBDImage>*> rgbdSubs_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
void RGBDXSync::rgbd2Callback(
const rtabmap_msgs::RGBDImageConstPtr& image0,
const rtabmap_msgs::RGBDImageConstPtr& image1)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(2);
@@ -209,7 +213,7 @@ void RGBDXSync::rgbd3Callback(
const rtabmap_msgs::RGBDImageConstPtr& image1,
const rtabmap_msgs::RGBDImageConstPtr& image2)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(3);
@@ -225,7 +229,7 @@ void RGBDXSync::rgbd4Callback(
const rtabmap_msgs::RGBDImageConstPtr& image2,
const rtabmap_msgs::RGBDImageConstPtr& image3)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(4);
@@ -243,7 +247,7 @@ void RGBDXSync::rgbd5Callback(
const rtabmap_msgs::RGBDImageConstPtr& image3,
const rtabmap_msgs::RGBDImageConstPtr& image4)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(5);
@@ -263,7 +267,7 @@ void RGBDXSync::rgbd6Callback(
const rtabmap_msgs::RGBDImageConstPtr& image4,
const rtabmap_msgs::RGBDImageConstPtr& image5)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(6);
@@ -285,7 +289,7 @@ void RGBDXSync::rgbd7Callback(
const rtabmap_msgs::RGBDImageConstPtr& image5,
const rtabmap_msgs::RGBDImageConstPtr& image6)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(7);
@@ -309,7 +313,7 @@ void RGBDXSync::rgbd8Callback(
const rtabmap_msgs::RGBDImageConstPtr& image6,
const rtabmap_msgs::RGBDImageConstPtr& image7)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(8);
rtabmap_sync/src/nodelets/stereo_sync.cpp
+6 -3
View File
@@ -56,7 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class StereoSync : public nodelet::Nodelet, public SyncDiagnostic
class StereoSync : public nodelet::Nodelet
{
public:
StereoSync() :
@@ -131,7 +131,8 @@ private:
cameraInfoRightSub_.getTopic().c_str());
NODELET_INFO(subscribedTopicsMsg.c_str());
initDiagnostic(left_nh.resolveName("image_rect"),
syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
syncDiagnostic_->init(left_nh.resolveName("image_rect"),
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
@@ -148,7 +149,7 @@ private:
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
tick(imageLeft->header.stamp);
syncDiagnostic_->tick(imageLeft->header.stamp);
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
@@ -240,6 +241,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_sync::StereoSync, nodelet::Nodelet);
rtabmap_util/src/MapAssemblerNode.cpp
+4 -4
View File
@@ -257,11 +257,11 @@ public:
rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
for(unsigned int i=0; i<msg.nodes.size(); ++i)
{
if(msg.nodes[i].image.size() ||
msg.nodes[i].depth.size() ||
msg.nodes[i].laserScan.size())
if(msg.nodes[i].data.left_compressed.size() ||
msg.nodes[i].data.right_compressed.size() ||
msg.nodes[i].data.laser_scan_compressed.size())
{
Signature data = rtabmap_conversions::nodeDataFromROS(msg.nodes[i]);
Signature data = rtabmap_conversions::nodeFromROS(msg.nodes[i]);
if(localGridsRegenerated_)
{
data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
rtabmap_util/src/MapOptimizerNode.cpp
+1 -1
View File
@@ -301,7 +301,7 @@ public:
for(std::list<int>::iterator iter=toAdd.begin(); iter!=toAdd.end(); ++iter)
{
UASSERT(cachedNodeInfos_.find(*iter) != cachedNodeInfos_.end());
rtabmap_conversions::nodeDataToROS(cachedNodeInfos_.at(*iter), outputDataMsg.nodes[oi]);
rtabmap_conversions::nodeToROS(cachedNodeInfos_.at(*iter), outputDataMsg.nodes[oi]);
++oi;
}
}
rtabmap_viz/include/rtabmap_viz/GuiWrapper.h
+5
View File
@@ -110,6 +110,11 @@ private:
const rtabmap_msgs::UserDataConstPtr & userDataMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg);
virtual void commonSensorDataCallback(
const rtabmap_msgs::SensorDataConstPtr & sensorDataMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg);
void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
void processRequestedMap(const rtabmap_msgs::MapData & map);
rtabmap_viz/src/GuiWrapper.cpp
+103
View File
@@ -1087,4 +1087,107 @@ void GuiWrapper::commonOdomCallback(
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
}
void GuiWrapper::commonSensorDataCallback(
const rtabmap_msgs::SensorDataConstPtr & sensorDataMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
{
UASSERT(sensorDataMsg.get());
std_msgs::Header odomHeader;
std::string frameId = frameId_;
if(odomMsg.get())
{
odomHeader = odomMsg->header;
if(!odomMsg->child_frame_id.empty())
{
frameId = odomMsg->child_frame_id;
}
else
{
ROS_WARN("Received odom topic with child_frame_id not set! Using \"%s\" as base frame.", frameId_.c_str());
}
}
else
{
odomHeader = sensorDataMsg->header;
odomHeader.frame_id = odomFrameId_;
}
Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
UASSERT(odomMsg->twist.covariance.size() == 36);
if(odomMsg->twist.covariance[0] != 0 &&
odomMsg->twist.covariance[7] != 0 &&
odomMsg->twist.covariance[14] != 0 &&
odomMsg->twist.covariance[21] != 0 &&
odomMsg->twist.covariance[28] != 0 &&
odomMsg->twist.covariance[35] != 0)
{
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone();
}
}
else if(odomInfoMsg.get() && odomInfoMsg->covariance.size() == 36)
{
if(odomInfoMsg->covariance[0] != 0 &&
odomInfoMsg->covariance[7] != 0 &&
odomInfoMsg->covariance[14] != 0 &&
odomInfoMsg->covariance[21] != 0 &&
odomInfoMsg->covariance[28] != 0 &&
odomInfoMsg->covariance[35] != 0)
{
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomInfoMsg->covariance.data()).clone();
}
}
if(odomHeader.frame_id.empty())
{
ROS_ERROR("Odometry frame not set!?");
return;
}
rtabmap::SensorData data;
rtabmap::OdometryInfo info;
bool ignoreData = false;
// limit update rate
if(maxOdomUpdateRate_<=0.0 ||
(UTimer::now() - lastOdomInfoUpdateTime_ > 1.0/maxOdomUpdateRate_ &&
!mainWindow_->isProcessingOdometry() &&
!mainWindow_->isProcessingStatistics()))
{
lastOdomInfoUpdateTime_ = UTimer::now();
data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg);
data.uncompressData();
if(odomInfoMsg.get())
{
info = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg);
}
ignoreData = false;
}
else if(odomInfoMsg.get())
{
data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg);
data.clearRawData();
data.clearCompressedData();
info = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
ignoreData = true;
}
else
{
// don't update GUI odom stuff if we don't use visual odometry
return;
}
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
data,
odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
}
}