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

Updated for RTAB-Map 0.18 (saving full scan info)

Browse Source
This commit is contained in:
matlabbe
2018-10-24 06:37:15 +12:00
parent bc53af5bbb
commit 2b8edffc85
5 changed files with 98 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+24 -8
View File
@@ -1209,10 +1209,18 @@ void CoreWrapper::commonDepthCallbackImpl(
userData_ = cv::Mat();
}
SensorData data(LaserScan::backwardCompatibility(scan,
scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0),
scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
scanLocalTransform),
SensorData data(scan2dMsg.get() !=0?
LaserScan::backwardCompatibility(scan,
scan2dMsg->range_min,
scan2dMsg->range_max,
scan2dMsg->angle_min,
scan2dMsg->angle_max,
scan2dMsg->angle_increment,
scanLocalTransform):
LaserScan::backwardCompatibility(scan,
genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
genScan_?genScanMaxDepth_:0.0f,
scanLocalTransform),
rgb,
depth,
cameraModels,
@@ -1468,10 +1476,18 @@ void CoreWrapper::commonStereoCallback(
userData_ = cv::Mat();
}
SensorData data(LaserScan::backwardCompatibility(scan,
scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
scan2dMsg.get() != 0?scan2dMsg->range_max:0,
scanLocalTransform),
SensorData data(scan2dMsg.get() != 0?
LaserScan::backwardCompatibility(scan,
scan2dMsg->range_min,
scan2dMsg->range_max,
scan2dMsg->angle_min,
scan2dMsg->angle_max,
scan2dMsg->angle_increment,
scanLocalTransform):
LaserScan::backwardCompatibility(scan,
scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
0,
scanLocalTransform),
left,
right,
stereoModel,
src/DbPlayerNode.cpp
+70 -39
View File
@@ -119,11 +119,10 @@ int main(int argc, char** argv)
pnh.param("start_id", startId, startId);
// A general 360 lidar with 0.5 deg increment
double scanAngleMin, scanAngleMax, scanAngleIncrement, scanTime, scanRangeMin, scanRangeMax;
double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax;
pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 720.0);
pnh.param<double>("scan_time", scanTime, 0);
pnh.param<double>("scan_range_min", scanRangeMin, 0.0);
pnh.param<double>("scan_range_max", scanRangeMax, 60);
@@ -170,6 +169,7 @@ int main(int argc, char** argv)
ros::Publisher rightCamInfoPub;
ros::Publisher odometryPub;
ros::Publisher scanPub;
ros::Publisher scanCloudPub;
ros::Publisher clockPub;
tf2_ros::TransformBroadcaster tfBroadcaster;
@@ -287,15 +287,27 @@ int main(int argc, char** argv)
if(!odom.data().laserScanRaw().isEmpty())
{
if(scanPub.getTopic().empty())
if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d())
{
scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
ROS_INFO("Scan will be published with those parameters:");
ROS_INFO(" scan_angle_min=%f", scanAngleMin);
ROS_INFO(" scan_angle_max=%f", scanAngleMax);
ROS_INFO(" scan_angle_increment=%f", scanAngleIncrement);
ROS_INFO(" scan_range_min=%f", scanRangeMin);
ROS_INFO(" scan_range_max=%f", scanRangeMax);
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
{
ROS_INFO("Scan will be published.");
}
else
{
ROS_INFO("Scan will be published with those parameters:");
ROS_INFO(" scan_angle_min=%f", scanAngleMin);
ROS_INFO(" scan_angle_max=%f", scanAngleMax);
ROS_INFO(" scan_angle_increment=%f", scanAngleIncrement);
ROS_INFO(" scan_range_min=%f", scanRangeMin);
ROS_INFO(" scan_range_max=%f", scanRangeMax);
}
}
else if(scanCloudPub.getTopic().empty())
{
scanCloudPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
ROS_INFO("Scan cloud will be published.");
}
}
@@ -331,7 +343,7 @@ int main(int argc, char** argv)
tfBroadcaster.sendTransform(odomToBase);
}
if(!scanPub.getTopic().empty())
if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty())
{
geometry_msgs::TransformStamped baseToLaserScan;
baseToLaserScan.child_frame_id = scanFrameId;
@@ -444,44 +456,63 @@ int main(int argc, char** argv)
rightCamInfoPub.publish(camInfoB);
}
if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().isEmpty())
if(!odom.data().laserScanRaw().isEmpty())
{
//inspired from pointcloud_to_laserscan package
sensor_msgs::LaserScan msg;
msg.header.frame_id = scanFrameId;
msg.header.stamp = time;
msg.angle_min = scanAngleMin;
msg.angle_max = scanAngleMax;
msg.angle_increment = scanAngleIncrement;
msg.time_increment = 0.0;
msg.scan_time = scanTime;
msg.range_min = scanRangeMin;
msg.range_max = scanRangeMax;
uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
msg.ranges.assign(rangesSize, 0.0);
const cv::Mat & scan = odom.data().laserScanRaw().data();
for (int i=0; i<scan.cols; ++i)
if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d())
{
const float * ptr = scan.ptr<float>(0,i);
double range = hypot(ptr[0], ptr[1]);
if (range >= scanRangeMin && range <=scanRangeMax)
//inspired from pointcloud_to_laserscan package
sensor_msgs::LaserScan msg;
msg.header.frame_id = scanFrameId;
msg.header.stamp = time;
msg.angle_min = scanAngleMin;
msg.angle_max = scanAngleMax;
msg.angle_increment = scanAngleIncrement;
msg.time_increment = 0.0;
msg.scan_time = 0;
msg.range_min = scanRangeMin;
msg.range_max = scanRangeMax;
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
{
double angle = atan2(ptr[1], ptr[0]);
if (angle >= msg.angle_min && angle <= msg.angle_max)
msg.angle_min = odom.data().laserScanRaw().angleMin();
msg.angle_max = odom.data().laserScanRaw().angleMax();
msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
msg.range_min = odom.data().laserScanRaw().rangeMin();
msg.range_max = odom.data().laserScanRaw().rangeMax();
}
uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
msg.ranges.assign(rangesSize, 0.0);
const cv::Mat & scan = odom.data().laserScanRaw().data();
for (int i=0; i<scan.cols; ++i)
{
const float * ptr = scan.ptr<float>(0,i);
double range = hypot(ptr[0], ptr[1]);
if (range >= msg.range_min && range <=msg.range_max)
{
int index = (angle - msg.angle_min) / msg.angle_increment;
if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
double angle = atan2(ptr[1], ptr[0]);
if (angle >= msg.angle_min && angle <= msg.angle_max)
{
msg.ranges[index] = range;
int index = (angle - msg.angle_min) / msg.angle_increment;
if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
{
msg.ranges[index] = range;
}
}
}
}
}
scanPub.publish(msg);
scanPub.publish(msg);
}
else if(scanCloudPub.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
msg.header.frame_id = scanFrameId;
msg.header.stamp = time;
scanCloudPub.publish(msg);
}
}
if(odom.data().userDataRaw().type() == CV_8SC1 &&
src/MsgConversion.cpp
+1 -1
View File
@@ -936,7 +936,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
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().maxRange();
msg.laserScanMaxRange = signature.sensorData().laserScanCompressed().rangeMax();
msg.laserScanFormat = signature.sensorData().laserScanCompressed().format();
transformToGeometryMsg(signature.sensorData().laserScanCompressed().localTransform(), msg.laserScanLocalTransform);
msg.baseline = 0;