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

Fixed build for upstream 0.16.1

Browse Source
This commit is contained in:
matlabbe
2018-02-16 20:00:32 -05:00
parent e3b2843d6f
commit 04b9abd1b9
12 changed files with 39 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+5 -7
View File
@@ -1033,7 +1033,7 @@ void CoreWrapper::commonDepthCallbackImpl(
if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
{
// backward compatibility, project 2D scan in /base_link frame
scan = util3d::transformLaserScan(scan, scanLocalTransform);
scan = util3d::transformLaserScan(LaserScan::backwardCompatibility(scan), scanLocalTransform).data();
scanLocalTransform = Transform::getIdentity();
}
}
@@ -1076,8 +1076,7 @@ void CoreWrapper::commonDepthCallbackImpl(
userData_ = cv::Mat();
}
SensorData data(scan,
LaserScanInfo(
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),
@@ -1280,7 +1279,7 @@ void CoreWrapper::commonStereoCallback(
if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
{
// backward compatibility, project 2D scan in /base_link frame
scan = util3d::transformLaserScan(scan, scanLocalTransform);
scan = util3d::transformLaserScan(LaserScan::backwardCompatibility(scan), scanLocalTransform).data();
scanLocalTransform = Transform::getIdentity();
}
}
@@ -1323,8 +1322,7 @@ void CoreWrapper::commonStereoCallback(
userData_ = cv::Mat();
}
SensorData data(scan,
LaserScanInfo(
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),
@@ -1451,7 +1449,7 @@ void CoreWrapper::process(
rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first ||
rtabmap_.getMemory()->getLastWorkingSignature() == 0 ||
rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0 ||
(!mapsManager_.getOccupancyGrid()->isGridFromDepth() && data.laserScanRaw().channels() == 2)) // 2d laser scan would fill empty space for latest data
(!mapsManager_.getOccupancyGrid()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data
{
SensorData tmpData = data;
tmpData.setId(-1);
src/DbPlayerNode.cpp
+6 -8
View File
@@ -262,7 +262,7 @@ int main(int argc, char** argv)
camInfoB.height = odom.data().depthOrRightRaw().rows;
camInfoB.width = odom.data().depthOrRightRaw().cols;
if(!odom.data().laserScanRaw().empty())
if(!odom.data().laserScanRaw().isEmpty())
{
if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
}
@@ -412,7 +412,7 @@ int main(int argc, char** argv)
rightCamInfoPub.publish(camInfoB);
}
if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().empty())
if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().isEmpty())
{
//inspired from pointcloud_to_laserscan package
sensor_msgs::LaserScan msg;
@@ -430,16 +430,14 @@ int main(int argc, char** argv)
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();
UASSERT(scan.type() == CV_32FC2 || scan.type() == CV_32FC3);
UASSERT(scan.rows == 1);
const cv::Mat & scan = odom.data().laserScanRaw().data();
for (int i=0; i<scan.cols; ++i)
{
cv::Vec2f pos = scan.at<cv::Vec2f>(i);
double range = hypot(pos[0], pos[1]);
const float * ptr = scan.ptr<float>(0,i);
double range = hypot(ptr[0], ptr[1]);
if (range >= scanRangeMin && range <=scanRangeMax)
{
double angle = atan2(pos[1], pos[0]);
double angle = atan2(ptr[1], ptr[0]);
if (angle >= msg.angle_min && angle <= msg.angle_max)
{
int index = (angle - msg.angle_min) / msg.angle_increment;
src/GuiWrapper.cpp
+2 -4
View File
@@ -572,8 +572,7 @@ void GuiWrapper::commonDepthCallback(
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
LaserScanInfo(
LaserScan::backwardCompatibility(scan,
scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
scan2dMsg.get()?(int)scan2dMsg->range_max:0,
scanLocalTransform),
@@ -728,8 +727,7 @@ void GuiWrapper::commonStereoCallback(
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
LaserScanInfo(
LaserScan::backwardCompatibility(scan,
scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
scan2dMsg.get()?(int)scan2dMsg->range_max:0,
scanLocalTransform),
src/MapsManager.cpp
+6 -4
View File
@@ -468,7 +468,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
cv::Mat ground, obstacles, emptyCells;
if(iter->first > 0)
{
cv::Mat rgb, depth, scan;
cv::Mat rgb, depth;
LaserScan scan;
bool generateGrid = data.gridCellSize() == 0.0f;
static bool warningShown = false;
if(occupancySavedInDB && generateGrid && !warningShown)
@@ -522,7 +523,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
occupancyGrid_->parseParameters(parameters);
}
cv::Mat rgb, depth, scan;
cv::Mat rgb, depth;
LaserScan scan;
bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_);
data.uncompressData(
occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
@@ -892,7 +894,7 @@ void MapsManager::publishMaps(
}
if(jter!=gridMaps_.end() && jter->second.first.first.cols)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.first.first, iter->second, 0, 255, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.first), iter->second, 0, 255, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
if(cloudSubtractFiltering_)
{
@@ -939,7 +941,7 @@ void MapsManager::publishMaps(
}
if(jter!=gridMaps_.end() && jter->second.first.second.cols)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.first.second, iter->second, 255, 0, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.second), iter->second, 255, 0, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
if(cloudSubtractFiltering_)
{
src/MsgConversion.cpp
+11 -10
View File
@@ -717,10 +717,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
transformFromPoseMsg(msg.groundTruthPose),
stereoModel.isValidForProjection()?
rtabmap::SensorData(
compressedMatFromBytes(msg.laserScan),
rtabmap::LaserScanInfo(
rtabmap::LaserScan(compressedMatFromBytes(msg.laserScan),
msg.laserScanMaxPts,
msg.laserScanMaxRange,
(rtabmap::LaserScan::Format)msg.laserScanFormat,
transformFromGeometryMsg(msg.laserScanLocalTransform)),
compressedMatFromBytes(msg.image),
compressedMatFromBytes(msg.depth),
@@ -729,10 +729,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
msg.stamp,
compressedMatFromBytes(msg.userData)):
rtabmap::SensorData(
compressedMatFromBytes(msg.laserScan),
rtabmap::LaserScanInfo(
rtabmap::LaserScan(compressedMatFromBytes(msg.laserScan),
msg.laserScanMaxPts,
msg.laserScanMaxRange,
(rtabmap::LaserScan::Format)msg.laserScanFormat,
transformFromGeometryMsg(msg.laserScanLocalTransform)),
compressedMatFromBytes(msg.image),
compressedMatFromBytes(msg.depth),
@@ -770,16 +770,17 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
msg.gps.bearing = signature.sensorData().gps().bearing();
compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image);
compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan);
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().laserScanInfo().maxPoints();
msg.laserScanMaxRange = signature.sensorData().laserScanInfo().maxRange();
transformToGeometryMsg(signature.sensorData().laserScanInfo().localTransform(), msg.laserScanLocalTransform);
msg.laserScanMaxPts = signature.sensorData().laserScanCompressed().maxPoints();
msg.laserScanMaxRange = signature.sensorData().laserScanCompressed().maxRange();
msg.laserScanFormat = signature.sensorData().laserScanCompressed().format();
transformToGeometryMsg(signature.sensorData().laserScanCompressed().localTransform(), msg.laserScanLocalTransform);
msg.baseline = 0;
if(signature.sensorData().cameraModels().size())
{
@@ -958,7 +959,7 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
}
info.localScanMap = rtabmap::uncompressData(msg.localScanMap);
info.localScanMap = rtabmap::LaserScan::backwardCompatibility(rtabmap::uncompressData(msg.localScanMap));
return info;
}
@@ -1009,7 +1010,7 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.localMapKeys = uKeys(info.localMap);
points3fToROS(uValues(info.localMap), msg.localMapValues);
msg.localScanMap = rtabmap::compressData(info.localScanMap);
msg.localScanMap = rtabmap::compressData(info.localScanMap.data());
}
cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg)
src/OdometryROS.cpp
+2 -2
View File
@@ -595,10 +595,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
}
}
if(odomLocalScanMap_.getNumSubscribers() && !info.localScanMap.empty())
if(odomLocalScanMap_.getNumSubscribers() && !info.localScanMap.isEmpty())
{
sensor_msgs::PointCloud2 cloudMsg;
if(info.localScanMap.channels() >= 5)
if(info.localScanMap.hasNormals())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap);
pcl::toROSMsg(*cloud, cloudMsg);
src/WifiSignalSubNode.cpp
+1
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/subscriber.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap_ros/MapData.h>
src/nodelets/icp_odometry.cpp
+2 -4
View File
@@ -232,8 +232,7 @@ private:
}
rtabmap::SensorData data(
scan,
LaserScanInfo(maxLaserScans, scanMsg->range_max, localScanTransform),
LaserScan::backwardCompatibility(scan, maxLaserScans, scanMsg->range_max, localScanTransform),
cv::Mat(),
cv::Mat(),
CameraModel(),
@@ -321,8 +320,7 @@ private:
}
rtabmap::SensorData data(
scan,
LaserScanInfo(maxLaserScans, 0, localScanTransform),
LaserScan::backwardCompatibility(scan, maxLaserScans, 0, localScanTransform),
cv::Mat(),
cv::Mat(),
CameraModel(),
src/nodelets/rgbdicp_odometry.cpp
+1 -2
View File
@@ -402,8 +402,7 @@ private:
}
rtabmap::SensorData data(
scan,
LaserScanInfo(
LaserScan::backwardCompatibility(scan,
scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0,
scanMsg.get() != 0?scanMsg->range_max:0,
localScanTransform),