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; }