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

Increased required rtabmap version to 0.20. Added ScanDescriptor and GlobalDescriptor msgs. rtabmap: added subscribe_scan_descriptor argument (updated common subscribers). RGBDImage.msg: added local keypoints, local points, local descriptors and global descriptor members. Info.msg: added wmState member.

Browse Source
This commit is contained in:
matlabbe
2020-05-03 22:42:01 -04:00
parent f034bf4e91
commit 7e283f9f1d
28 changed files with 2853 additions and 667 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GuiWrapper.cpp
+40 -33
View File
@@ -425,9 +425,13 @@ void GuiWrapper::commonDepthCallback(
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
const sensor_msgs::LaserScan& scan2dMsg,
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
const std::vector<cv::Mat> & localDescriptors)
{
UASSERT(imageMsgs.size() == 0 || (imageMsgs.size() == cameraInfoMsgs.size()));
@@ -438,13 +442,13 @@ void GuiWrapper::commonDepthCallback(
}
else
{
if(scan2dMsg.get())
if(!scan2dMsg.ranges.empty())
{
odomHeader = scan2dMsg->header;
odomHeader = scan2dMsg.header;
}
else if(scan3dMsg.get())
else if(!scan3dMsg.data.empty())
{
odomHeader = scan3dMsg->header;
odomHeader = scan3dMsg.header;
}
else if(cameraInfoMsgs.size())
{
@@ -529,7 +533,7 @@ void GuiWrapper::commonDepthCallback(
}
}
if(scan2dMsg.get() != 0)
if(!scan2dMsg.ranges.empty())
{
if(!rtabmap_ros::convertScanMsg(
scan2dMsg,
@@ -544,7 +548,7 @@ void GuiWrapper::commonDepthCallback(
return;
}
}
else if(scan3dMsg.get() != 0)
else if(!scan3dMsg.data.empty())
{
if(!rtabmap_ros::convertScan3dMsg(
scan3dMsg,
@@ -599,9 +603,13 @@ void GuiWrapper::commonStereoCallback(
const cv_bridge::CvImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfo& leftCamInfoMsg,
const sensor_msgs::CameraInfo& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
const sensor_msgs::LaserScan& scan2dMsg,
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
const std::vector<cv::Mat> & localDescriptors)
{
std_msgs::Header odomHeader;
if(odomMsg.get())
@@ -610,13 +618,13 @@ void GuiWrapper::commonStereoCallback(
}
else
{
if(scan2dMsg.get())
if(!scan2dMsg.ranges.empty())
{
odomHeader = scan2dMsg->header;
odomHeader = scan2dMsg.header;
}
else if(scan3dMsg.get())
else if(!scan3dMsg.data.empty())
{
odomHeader = scan3dMsg->header;
odomHeader = scan3dMsg.header;
}
else
{
@@ -691,7 +699,7 @@ void GuiWrapper::commonStereoCallback(
return;
}
if(scan2dMsg.get() != 0)
if(!scan2dMsg.ranges.empty())
{
if(!rtabmap_ros::convertScanMsg(
scan2dMsg,
@@ -706,7 +714,7 @@ void GuiWrapper::commonStereoCallback(
return;
}
}
else if(scan3dMsg.get() != 0)
else if(!scan3dMsg.data.empty())
{
if(!rtabmap_ros::convertScan3dMsg(
scan3dMsg,
@@ -757,12 +765,11 @@ void GuiWrapper::commonStereoCallback(
void GuiWrapper::commonLaserScanCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
const sensor_msgs::LaserScan& scan2dMsg,
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
const rtabmap_ros::GlobalDescriptor & globalDescriptor)
{
UASSERT(scan2dMsg.get() || scan3dMsg.get());
std_msgs::Header odomHeader;
if(odomMsg.get())
{
@@ -770,13 +777,13 @@ void GuiWrapper::commonLaserScanCallback(
}
else
{
if(scan2dMsg.get())
if(!scan2dMsg.ranges.empty())
{
odomHeader = scan2dMsg->header;
odomHeader = scan2dMsg.header;
}
else if(scan3dMsg.get())
else if(!scan3dMsg.data.empty())
{
odomHeader = scan3dMsg->header;
odomHeader = scan3dMsg.header;
}
else
{
@@ -831,7 +838,7 @@ void GuiWrapper::commonLaserScanCallback(
{
lastOdomInfoUpdateTime_ = UTimer::now();
if(scan2dMsg.get() != 0)
if(!scan2dMsg.ranges.empty())
{
if(!rtabmap_ros::convertScanMsg(
scan2dMsg,
@@ -846,7 +853,7 @@ void GuiWrapper::commonLaserScanCallback(
return;
}
}
else if(scan3dMsg.get() != 0)
else if(!scan3dMsg.data.empty())
{
if(!rtabmap_ros::convertScan3dMsg(
scan3dMsg,
@@ -871,13 +878,13 @@ void GuiWrapper::commonLaserScanCallback(
else if(odomInfoMsg.get())
{
//just get scan local transform to adjust camera frame
if(scan2dMsg.get() != 0)
if(!scan2dMsg.ranges.empty())
{
fakeCameraLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
fakeCameraLocalTransform = getTransform(frameId_, scan2dMsg.header.frame_id, scan2dMsg.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
}
else if(scan3dMsg.get() != 0)
else if(!scan3dMsg.data.empty())
{
fakeCameraLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
fakeCameraLocalTransform = getTransform(frameId_, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
}
info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();