void CObservation2DRangeScan_from_ROS_LaserScan_msg(
CObservation2DRangeScan& self, object scan_msg, CPose3D pose)
{
// set info
self.sensorLabel =
extract<std::string>(scan_msg.attr("header").attr("frame_id"));
auto t = mrpt::Clock::fromDouble(extract<uint64_t>(
TTimeStamp_from_ROS_Time(scan_msg.attr("header").attr("stamp"))));
self.timestamp = t;
self.maxRange = extract<float>(scan_msg.attr("range_max"));
self.aperture = extract<float>(scan_msg.attr("angle_max")) -
extract<float>(scan_msg.attr("angle_min"));
self.beamAperture = extract<float>(scan_msg.attr("angle_increment"));
self.sensorPose = pose;
// set ranges
tuple ranges = extract<tuple>(scan_msg.attr("ranges"));
const size_t N = len(ranges);
self.resizeScan(N);
for (int i = 0; i < len(ranges); ++i)
{
float range = extract<float>(ranges[i]);
self.setScanRange(i, range);
self.setScanRangeValidity(i, (range < self.maxRange - 0.01));
}
}
bool CLMS100Eth::decodeScan(char* buff, CObservation2DRangeScan& outObservation)
{
char* next;
unsigned int idx = 0;
unsigned int scanCount = 0;
char* tmp;
// double factor;
next = strtok(buff, " ", &tmp);
while (next && scanCount == 0)
{
// cout << "Interpreting : " << next << endl;
switch (++idx)
{
case 1:
if (strncmp(&next[1], "sRA", 3) && strncmp(&next[1], "sSN", 3))
return false;
break;
case 2:
if (strcmp(next, "LMDscandata")) return false;
break;
case 6:
if (!strcmp(next, "1"))
{
MRPT_LOG_ERROR_FMT("STATUS error on LMS100: '%s'", next);
}
else if (!strcmp(next, "4"))
{
MRPT_LOG_ERROR_FMT(
"Contamination error on LMS100: '%s'", next);
}
else
{
MRPT_LOG_DEBUG("STATUS Ok.\n");
}
break;
case 21:
if (strcmp(next, "DIST1"))
{
THROW_EXCEPTION(
"LMS100 is not configured to send distances.");
return false;
}
MRPT_LOG_DEBUG("Distance : OK\n");
break;
case 22:
// factor = strtod(next, nullptr);
break;
case 26:
scanCount = strtoul(next, nullptr, 16);
MRPT_LOG_DEBUG_FMT("Scan Count : %d\n", scanCount);
break;
default:
break;
}
next = strtok(nullptr, " ", &tmp);
}
outObservation.aperture = (float)APPERTURE;
outObservation.rightToLeft = false;
outObservation.stdError = 0.012f;
outObservation.sensorPose = m_sensorPose;
outObservation.beamAperture = m_beamApperture;
outObservation.maxRange = m_maxRange;
outObservation.timestamp = mrpt::system::getCurrentTime();
outObservation.sensorLabel = m_sensorLabel;
outObservation.resizeScan(scanCount);
unsigned int i;
for (i = 0; i < scanCount && next; i++, next = strtok(nullptr, " ", &tmp))
{
outObservation.setScanRange(
i, double(strtoul(next, nullptr, 16)) / 1000.0);
outObservation.setScanRangeValidity(
i, outObservation.getScanRange(i) <= outObservation.maxRange);
}
outObservation.resizeScan(i);
return i >= scanCount;
}
// CSensoryFrame
void CSensoryFrame_insert1(CSensoryFrame &self, CObservation2DRangeScan &obs)
{
CObservationPtr obsPtr = (CObservationPtr) obs.duplicateGetSmartPtr();
self.insert(obsPtr);
}
