Skip to content
Permalink
Browse files

rosbag: convert pointcloud to new rotating scan object

  • Loading branch information...
jlblancoc committed Sep 9, 2019
1 parent 02a26b9 commit 3c14214f7b5560258910cddd2cffed92252672ca
@@ -22,6 +22,7 @@
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/obs/CObservationIMU.h>
#include <mrpt/obs/CObservationPointCloud.h>
#include <mrpt/obs/CObservationRotatingScan.h>
#include <mrpt/otherlibs/tclap/CmdLine.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/serialization/CArchive.h>
@@ -209,27 +210,40 @@ Obs toPointCloud2(std::string_view msg, const rosbag::MessageInstance& rosmsg)
if (!fields.count("x") || !fields.count("y") || !fields.count("z"))
return {};

#if 0
if (fields.count("ring"))
{
// As a structured Velodyne observation with ring number:
MRPT_TODO("Implement me!");
}
else
#endif
if (fields.count("intensity"))
if (fields.count("ring"))
{
// As a structured 2D range images, if we have ring numbers:
auto obsRotScan = mrpt::obs::CObservationRotatingScan::Create();
MRPT_TODO("Extract sensor pose from tf frames");
const mrpt::poses::CPose3D sensorPose;

if (!mrpt::ros1bridge::fromROS(*pts, *obsRotScan, sensorPose))
THROW_EXCEPTION(
"Could not convert pointcloud from ROS to "
"CObservationRotatingScan");

obsRotScan->sensorLabel = msg;
return {obsRotScan};
}
else if (fields.count("intensity"))
{
// XYZI
auto mrptPts = mrpt::maps::CPointsMapXYZI::Create();
ptsObs->pointcloud = mrptPts;
mrpt::ros1bridge::fromROS(*pts, *mrptPts);
if (!mrpt::ros1bridge::fromROS(*pts, *mrptPts))
THROW_EXCEPTION(
"Could not convert pointcloud from ROS to "
"CPointsMapXYZI");
}
else
{
// XYZ
auto mrptPts = mrpt::maps::CSimplePointsMap::Create();
ptsObs->pointcloud = mrptPts;
mrpt::ros1bridge::fromROS(*pts, *mrptPts);
if (!mrpt::ros1bridge::fromROS(*pts, *mrptPts))
THROW_EXCEPTION(
"Could not convert pointcloud from ROS to "
"CSimplePointsMap");
}

return {ptsObs};
@@ -38,7 +38,7 @@ uint8_t RotScan::serializeGetVersion() const { return 0; }
void RotScan::serializeTo(mrpt::serialization::CArchive& out) const
{
out << timestamp << sensorLabel << rowCount << columnCount
<< rangeResolution << startAzimuth << azimuthSpan << sweepDuration
<< rangeResolution << startAzimuth << azimuthSpan << sweepDuration
<< lidarModel << minRange << maxRange << sensorPose
<< originalReceivedTimestamp << has_satellite_timestamp;

@@ -70,7 +70,7 @@ void RotScan::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
case 0:
{
in >> timestamp >> sensorLabel >> rowCount >> columnCount >>
rangeResolution >> startAzimuth >> azimuthSpan >>
rangeResolution >> startAzimuth >> azimuthSpan >>
sweepDuration >> lidarModel >> minRange >> maxRange >>
sensorPose >> originalReceivedTimestamp >>
has_satellite_timestamp;
@@ -170,11 +170,11 @@ void RotScan::fromVelodyne(const mrpt::obs::CObservationVelodyneScan& o)

// row count:
columnCount =
Velo::SCANS_PER_BLOCK * o.scan_packets.size() * Velo::BLOCKS_PER_PACKET;
Velo::SCANS_PER_BLOCK * o.scan_packets.size() * Velo::BLOCKS_PER_PACKET;

const double timeBetweenLastTwoBlocks =
1e-6 * (o.scan_packets.rbegin()->gps_timestamp -
(o.scan_packets.rbegin() + 1)->gps_timestamp);
1e-6 * (o.scan_packets.rbegin()->gps_timestamp -
(o.scan_packets.rbegin() + 1)->gps_timestamp);

rangeImage.setZero(rowCount, columnCount);
intensityImage.setZero(rowCount, columnCount);
@@ -212,7 +212,7 @@ void RotScan::fromVelodyne(const mrpt::obs::CObservationVelodyneScan& o)
// last packet:
// sanity checks: rot speed should be fairly stable:
const double maxRotSpeed = *rotspeed.rbegin(),
minRotSpeed = *rotspeed.begin();
minRotSpeed = *rotspeed.begin();
ASSERT_ABOVE_(maxRotSpeed, 0);
ASSERT_BELOW_((maxRotSpeed - minRotSpeed) / maxRotSpeed, 0.01);

@@ -224,38 +224,38 @@ void RotScan::fromVelodyne(const mrpt::obs::CObservationVelodyneScan& o)
}();

azimuthSpan +=
mrpt::DEG2RAD(rotVel_degps * timeBetweenLastTwoBlocks);
mrpt::DEG2RAD(rotVel_degps * timeBetweenLastTwoBlocks);
}
else
{
// non-last packet:
const double curAng =
0.01 * o.scan_packets[pktIdx].blocks[0].rotation;
0.01 * o.scan_packets[pktIdx].blocks[0].rotation;
const double nextAng =
0.01 * o.scan_packets[pktIdx + 1].blocks[0].rotation;
0.01 * o.scan_packets[pktIdx + 1].blocks[0].rotation;

const double incrAng = mrpt::math::angDistance(
mrpt::DEG2RAD(curAng), mrpt::DEG2RAD(nextAng));
mrpt::DEG2RAD(curAng), mrpt::DEG2RAD(nextAng));
azimuthSpan += incrAng;
}

// Process each block in this packet:
for (int block = 0; block < Velo::BLOCKS_PER_PACKET; block++)
{
const int dsr_offset =
(pkt.blocks[block].header == Velo::LOWER_BANK) ? 32 : 0;
(pkt.blocks[block].header == Velo::LOWER_BANK) ? 32 : 0;
const bool block_is_dual_strongest_range =
(pkt.laser_return_mode == Velo::RETMODE_DUAL &&
((block & 0x01) != 0));
(pkt.laser_return_mode == Velo::RETMODE_DUAL &&
((block & 0x01) != 0));
const bool block_is_dual_last_range =
(pkt.laser_return_mode == Velo::RETMODE_DUAL &&
((block & 0x01) == 0));
(pkt.laser_return_mode == Velo::RETMODE_DUAL &&
((block & 0x01) == 0));

for (int dsr = 0, k = 0; dsr < Velo::SCANS_PER_FIRING; dsr++, k++)
{
if (!pkt.blocks[block]
.laser_returns[k]
.distance) // Invalid return?
.laser_returns[k]
.distance) // Invalid return?
continue;

const auto rawLaserId = static_cast<uint8_t>(dsr + dsr_offset);
@@ -279,43 +279,43 @@ void RotScan::fromVelodyne(const mrpt::obs::CObservationVelodyneScan& o)
// ignore one of them:

const auto distance =
pkt.blocks[block].laser_returns[k].distance +
static_cast<uint16_t>(
calib.distanceCorrection / Velo::DISTANCE_RESOLUTION);
pkt.blocks[block].laser_returns[k].distance +
static_cast<uint16_t>(
calib.distanceCorrection / Velo::DISTANCE_RESOLUTION);

const auto columnIdx = [&]() {
switch (num_lasers)
{
case 16:
case 32:
case 64:
{
int c = (dsr + block * Velo::SCANS_PER_BLOCK +
pktIdx * Velo::SCANS_PER_PACKET) /
num_lasers;
case 16:
case 32:
case 64:
{
int c = (dsr + block * Velo::SCANS_PER_BLOCK +
pktIdx * Velo::SCANS_PER_PACKET) /
num_lasers;
if (pkt.laser_return_mode == Velo::RETMODE_DUAL)
c /= 2;
return c;
}
default:
{
THROW_EXCEPTION("Error: unhandled LIDAR model!");
}
}
default:
{
THROW_EXCEPTION("Error: unhandled LIDAR model!");
}
};
}();

std::cout << int(laserId) << " " << columnIdx << "\n";

ASSERT_BELOW_(columnIdx, columnCount);
if (pkt.laser_return_mode != Velo::RETMODE_DUAL ||
block_is_dual_strongest_range)
block_is_dual_strongest_range)
{
// Regular range, or strongest in multi return mode:
rangeImage(laserId, columnIdx) = distance;

// Intensity:
intensityImage(laserId, columnIdx) =
pkt.blocks[block].laser_returns[k].intensity;
pkt.blocks[block].laser_returns[k].intensity;
}
else if (block_is_dual_last_range)
{
@@ -334,25 +334,25 @@ void RotScan::fromVelodyne(const mrpt::obs::CObservationVelodyneScan& o)

// Start and end azimuth:
startAzimuth =
mrpt::DEG2RAD(o.scan_packets.begin()->blocks[0].rotation * 1e-2);
mrpt::DEG2RAD(o.scan_packets.begin()->blocks[0].rotation * 1e-2);

const auto microsecs_1st_pkt = o.scan_packets.begin()->gps_timestamp;
const auto microsecs_last_pkt = o.scan_packets.rbegin()->gps_timestamp;
sweepDuration = 1e-6 * (microsecs_last_pkt - microsecs_1st_pkt) +
timeBetweenLastTwoBlocks;
timeBetweenLastTwoBlocks;

// Decode model byte:
switch (model)
{
case 0x21:
lidarModel = "HDL-32E";
break;
case 0x22:
lidarModel = "VLP-16";
break;
default:
lidarModel = "Unknown";
break;
case 0x21:
lidarModel = "HDL-32E";
break;
case 0x22:
lidarModel = "VLP-16";
break;
default:
lidarModel = "Unknown";
break;
};

// rangeImage.saveToTextFile("/tmp/range.txt");
@@ -42,7 +42,7 @@ TEST(CObservationRotatingScan, fromVelodyne)
{
using namespace std::string_literals;
const auto fil = mrpt::system::getShareMRPTDir() +
"/datasets/test_velodyne_VLP16.rawlog"s;
"/datasets/test_velodyne_VLP16.rawlog"s;

mrpt::obs::CRawlog rawlog;
bool load_ok = rawlog.loadFromRawLogFile(fil);
@@ -12,6 +12,7 @@
#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/maps/CPointsMapXYZI.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservationRotatingScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <set>
#include <string>
@@ -27,16 +28,27 @@ namespace mrpt::ros1bridge
/** Convert sensor_msgs/PointCloud2 -> mrpt::slam::CSimplePointsMap
* Only (x,y,z) data is converted. To use the intensity channel, see
* the alternative signature for CPointsMapXYZI.
* Requires point cloud fields: x,y,z.
* \return true on sucessful conversion, false on any error.
* \sa toROS
*/
bool fromROS(
const sensor_msgs::PointCloud2& msg, mrpt::maps::CSimplePointsMap& obj);

/** \overload For (x,y,z,intensity) channels. */
/** \overload For (x,y,z,intensity) channels.
* Requires point cloud fields: x,y,z,intensity
*/
bool fromROS(
const sensor_msgs::PointCloud2& msg, mrpt::maps::CPointsMapXYZI& obj);

/** Convert sensor_msgs/PointCloud2 -> mrpt::obs::CObservationRotatingScan.
* Requires point cloud fields: x,y,z,intensity,ring
*/
bool fromROS(
const sensor_msgs::PointCloud2& m, mrpt::obs::CObservationRotatingScan& o,
const mrpt::poses::CPose3D& sensorPoseOnRobot,
unsigned int num_azimuth_divisions = 360);

/** Extract a list of fields found in the point cloud.
* Typically: {"x","y","z","intensity"}
*/

0 comments on commit 3c14214

Please sign in to comment.
You can’t perform that action at this time.