bool HectorMappingRos::rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap)
{
size_t size = scan.ranges.size();
float angle = scan.angle_min;
dataContainer.clear();
dataContainer.setOrigo(Eigen::Vector2f::Zero());
float maxRangeForContainer = scan.range_max - 0.1f;
for (size_t i = 0; i < size; ++i)
{
float dist = scan.ranges[i];
if ( (dist > scan.range_min) && (dist < maxRangeForContainer))
{
dist *= scaleToMap;
dataContainer.add(Eigen::Vector2f(cos(angle) * dist, sin(angle) * dist));
}
angle += scan.angle_increment;
}
return true;
}
bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap)
{
size_t size = pointCloud.points.size();
dataContainer.clear();
tf::Vector3 laserPos (laserTransform.getOrigin());
dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y())*scaleToMap);
for (size_t i = 0; i < size; ++i)
{
const geometry_msgs::Point32& currPoint(pointCloud.points[i]);
float dist_sqr = currPoint.x*currPoint.x + currPoint.y* currPoint.y;
if ( (dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_) ){
if ( (currPoint.x < 0.0f) && (dist_sqr < 0.50f)){
continue;
}
tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z));
float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z();
if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_)
{
dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(),pointPosBaseFrame.y())*scaleToMap);
}
}
}
return true;
}