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; }
karto::LaserRangeFinder* SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan) { // Check whether we know about this laser yet if(lasers_.find(scan->header.frame_id) == lasers_.end()) { // New laser; need to create a Karto device for it. // 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); } catch(tf::TransformException e) { ROS_WARN("Failed to compute laser pose, aborting initialization (%s)", e.what()); return NULL; } double yaw = tf::getYaw(laser_pose.getRotation()); ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f", scan->header.frame_id.c_str(), 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; } double angle_min = tf::getYaw(min_q); double angle_max = tf::getYaw(max_q); bool inverse = lasers_inverted_[scan->header.frame_id] = angle_max < angle_min; if (inverse) ROS_INFO("laser is mounted upside-down"); // Create a laser range finder device and copy in data from the first // scan std::string name = scan->header.frame_id; karto::LaserRangeFinder* laser = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name)); laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), yaw)); laser->SetMinimumRange(scan->range_min); laser->SetMaximumRange(scan->range_max); laser->SetMinimumAngle(scan->angle_min); laser->SetMaximumAngle(scan->angle_max); laser->SetAngularResolution(scan->angle_increment); // TODO: expose this, and many other parameters //laser_->SetRangeThreshold(12.0); // Store this laser device for later lasers_[scan->header.frame_id] = laser; // Add it to the dataset, which seems to be necessary dataset_->Add(laser); } return lasers_[scan->header.frame_id]; }