bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan) { laser_frame_ = scan.header.frame_id; // Get the laser's pose, relative to base. tf::Stamped<tf::Pose> ident; tf::Stamped<tf::Transform> laser_pose; ident.setIdentity(); ident.frame_id_ = laser_frame_; ident.stamp_ = scan.header.stamp; try { tf_.transformPose(base_frame_, ident, laser_pose); } catch(tf::TransformException e) { ROS_WARN("Failed to compute laser pose, aborting initialization (%s)", e.what()); return false; } // create a point 1m above the laser position and transform it into the laser-frame tf::Vector3 v; v.setValue(0, 0, 1 + laser_pose.getOrigin().z()); tf::Stamped<tf::Vector3> up(v, scan.header.stamp, base_frame_); try { tf_.transformPoint(laser_frame_, up, up); ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z()); } catch(tf::TransformException& e) { ROS_WARN("Unable to determine orientation of laser: %s", e.what()); return false; } // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment. if (fabs(fabs(up.z()) - 1) > 0.001) { ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f", up.z()); return false; } gsp_laser_beam_count_ = scan.ranges.size(); int orientationFactor; if (up.z() > 0) { orientationFactor = 1; ROS_INFO("Laser is mounted upwards."); } else { orientationFactor = -1; ROS_INFO("Laser is mounted upside down."); } angle_min_ = orientationFactor * scan.angle_min; angle_max_ = orientationFactor * scan.angle_max; gsp_laser_angle_increment_ = orientationFactor * scan.angle_increment; ROS_DEBUG("Laser angles top down in laser-frame: min: %.3f max: %.3f inc: %.3f", angle_min_, angle_max_, gsp_laser_angle_increment_); GMapping::OrientedPoint gmap_pose(0, 0, 0); // setting maxRange and maxUrange here so we can set a reasonable default ros::NodeHandle private_nh_("~"); if(!private_nh_.getParam("maxRange", maxRange_)) maxRange_ = scan.range_max - 0.01; if(!private_nh_.getParam("maxUrange", maxUrange_)) maxUrange_ = maxRange_; // The laser must be called "FLASER". // We pass in the absolute value of the computed angle increment, on the // assumption that GMapping requires a positive angle increment. If the // actual increment is negative, we'll swap the order of ranges before // feeding each scan to GMapping. gsp_laser_ = new GMapping::RangeSensor("FLASER", gsp_laser_beam_count_, fabs(gsp_laser_angle_increment_), gmap_pose, 0.0, maxRange_); ROS_ASSERT(gsp_laser_); GMapping::SensorMap smap; smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_)); gsp_->setSensorMap(smap); gsp_odom_ = new GMapping::OdometrySensor(odom_frame_); ROS_ASSERT(gsp_odom_); /// @todo Expose setting an initial pose GMapping::OrientedPoint initialPose; if(!getOdomPose(initialPose, scan.header.stamp)) { ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero."); initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0); } gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_, kernelSize_, lstep_, astep_, iterations_, lsigma_, ogain_, lskip_); gsp_->setMotionModelParameters(srr_, srt_, str_, stt_); gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_); gsp_->setUpdatePeriod(temporalUpdate_); gsp_->setgenerateMap(false); gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_, delta_, initialPose); gsp_->setllsamplerange(llsamplerange_); gsp_->setllsamplestep(llsamplestep_); /// @todo Check these calls; in the gmapping gui, they use /// llsamplestep and llsamplerange intead of lasamplestep and /// lasamplerange. It was probably a typo, but who knows. gsp_->setlasamplerange(lasamplerange_); gsp_->setlasamplestep(lasamplestep_); // Call the sampling function once to set the seed. GMapping::sampleGaussian(1,time(NULL)); ROS_INFO("Initialization complete"); return true; }
bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan) { // Get the laser's pose, relative to base. tf::Stamped<tf::Pose> ident; tf::Stamped<tf::Transform> laser_pose; ident.setIdentity(); ident.frame_id_ = scan.header.frame_id; ident.stamp_ = scan.header.stamp; try { tf_.transformPose(base_frame_, ident, laser_pose); #ifdef LOGTOFILE writeLaserPoseStamped(laser_pose); #endif } catch(tf::TransformException e) { ROS_WARN("Failed to compute laser pose, aborting initialization (%s)", e.what()); return false; } double yaw = tf::getYaw(laser_pose.getRotation()); GMapping::OrientedPoint gmap_pose(laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), yaw); ROS_DEBUG("laser's pose wrt base: %.3f %.3f %.3f", laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), yaw); // To account for lasers that are mounted upside-down, we determine the // min, max, and increment angles of the laser in the base frame. tf::Quaternion q; q.setRPY(0.0, 0.0, scan.angle_min); tf::Stamped<tf::Quaternion> min_q(q, scan.header.stamp, scan.header.frame_id); q.setRPY(0.0, 0.0, scan.angle_max); tf::Stamped<tf::Quaternion> max_q(q, scan.header.stamp, scan.header.frame_id); try { tf_.transformQuaternion(base_frame_, min_q, min_q); tf_.transformQuaternion(base_frame_, max_q, max_q); } catch(tf::TransformException& e) { ROS_WARN("Unable to transform min/max laser angles into base frame: %s", e.what()); return false; } #ifdef LOGTOFILE writeMinMax(min_q); writeMinMax(max_q); #endif gsp_laser_beam_count_ = scan.ranges.size(); double angle_min = tf::getYaw(min_q); double angle_max = tf::getYaw(max_q); gsp_laser_angle_increment_ = scan.angle_increment; ROS_DEBUG("Laser angles in base frame: min: %.3f max: %.3f inc: %.3f", angle_min, angle_max, gsp_laser_angle_increment_); // setting maxRange and maxUrange here so we can set a reasonable default ros::NodeHandle private_nh_("~"); if(!private_nh_.getParam("maxRange", maxRange_)) maxRange_ = scan.range_max - 0.01; if(!private_nh_.getParam("maxUrange", maxUrange_)) maxUrange_ = maxRange_; // The laser must be called "FLASER". // We pass in the absolute value of the computed angle increment, on the // assumption that GMapping requires a positive angle increment. If the // actual increment is negative, we'll swap the order of ranges before // feeding each scan to GMapping. gsp_laser_ = new GMapping::RangeSensor("FLASER", gsp_laser_beam_count_, fabs(gsp_laser_angle_increment_), gmap_pose, 0.0, maxRange_); ROS_ASSERT(gsp_laser_); GMapping::SensorMap smap; smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_)); gsp_->setSensorMap(smap); gsp_odom_ = new GMapping::OdometrySensor(odom_frame_); ROS_ASSERT(gsp_odom_); /// @todo Expose setting an initial pose GMapping::OrientedPoint initialPose; if(!getOdomPose(initialPose, scan.header.stamp)) initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0); gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_, kernelSize_, lstep_, astep_, iterations_, lsigma_, ogain_, lskip_); gsp_->setMotionModelParameters(srr_, srt_, str_, stt_); gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_); gsp_->setUpdatePeriod(temporalUpdate_); gsp_->setgenerateMap(false); gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_, delta_, initialPose); gsp_->setllsamplerange(llsamplerange_); gsp_->setllsamplestep(llsamplestep_); /// @todo Check these calls; in the gmapping gui, they use /// llsamplestep and llsamplerange intead of lasamplestep and /// lasamplerange. It was probably a typo, but who knows. gsp_->setlasamplerange(lasamplerange_); gsp_->setlasamplestep(lasamplestep_); // Call the sampling function once to set the seed. GMapping::sampleGaussian(1,time(NULL)); ROS_INFO("Initialization complete"); return true; }