bool
SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{
if(!getOdomPose(gmap_pose, scan.header.stamp))
return false;
if(scan.ranges.size() != gsp_laser_beam_count_)
return false;
// GMapping wants an array of doubles...
double* ranges_double = new double[scan.ranges.size()];
// If the angle increment is negative, we have to invert the order of the readings.
if (do_reverse_range_)
{
ROS_DEBUG("Inverting scan");
int num_ranges = scan.ranges.size();
for(int i=0; i < num_ranges; i++)
{
// Must filter out short readings, because the mapper won't
if(scan.ranges[num_ranges - i - 1] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
}
} else
{
for(unsigned int i=0; i < scan.ranges.size(); i++)
{
// Must filter out short readings, because the mapper won't
if(scan.ranges[i] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[i];
}
}
GMapping::RangeReading reading(scan.ranges.size(),
ranges_double,
gsp_laser_,
scan.header.stamp.toSec());
// ...but it deep copies them in RangeReading constructor, so we don't
// need to keep our array around.
delete[] ranges_double;
reading.setPose(gmap_pose);
/*
ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",
scan.header.stamp.toSec(),
gmap_pose.x,
gmap_pose.y,
gmap_pose.theta);
*/
ROS_DEBUG("processing scan");
return gsp_->processScan(reading);
}
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::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{
if(!getOdomPose(gmap_pose, scan.header.stamp))
return false;
if(scan.ranges.size() != gsp_laser_beam_count_)
return false;
// GMapping wants an array of doubles...
double* ranges_double = new double[scan.ranges.size()];
// If the angle increment is negative, then we conclude that the laser is
// upside down, and invert the order of the readings.
if (gsp_laser_angle_increment_ < 0)
{
ROS_DEBUG("Inverting scan");
int num_ranges = scan.ranges.size();
for(int i=0; i < num_ranges; i++)
{
// Must filter out short readings, because the mapper won't
if(scan.ranges[i] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
}
} else
{
for(unsigned int i=0; i < scan.ranges.size(); i++)
{
// Must filter out short readings, because the mapper won't
if(scan.ranges[i] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[i];
}
}
GMapping::RangeReading reading(scan.ranges.size(),
ranges_double,
gsp_laser_,
scan.header.stamp.toSec());
// ...but it deep copies them in RangeReading constructor, so we don't
// need to keep our array around.
delete[] ranges_double;
reading.setPose(gmap_pose);
//#######################write particle to file #############################3
// if (write_particles_to_file==2){
// {
// using namespace google::protobuf::io;
// // Write the new address book back to disk.
// std::fstream output("workpackage", std::ios::out | std::ios::trunc | std::ios::binary);
// /*google::protobuf::io::OstreamOutputStream file_stream((std::ostream *) &output);
// GzipOutputStream::Options options;
// options.format = GzipOutputStream::GZIP;
// options.compression_level = 2;
// google::protobuf::io::GzipOutputStream gzip_stream(&file_stream,
// options);*/
// ProtoBuf::ProtobufHelper pbufhelper;
// pbufhelper.initWorkPackage(0,gsp_->m_matcher);
// const GMapping::ParticleVector& pvector = gsp_->getParticles();
// pbufhelper.addParticleToWorkPackage(pvector[0],0);
// pbufhelper.setWorkPackageLaserReading(reading);
// /*if (!pbufparticle.SerializeToZeroCopyStream(&gzip_stream)) {
// std::cerr << "Failed to write particle." << std::endl;
// return -1;
// }*/
// if (!pbufhelper.m_workpackage.SerializeToOstream(&output)) {
// std::cerr << "Failed to write workpackage." << std::endl;
// return -1;
// }
// ROS_INFO("Writing workpackage ################################");
// }
// write_particles_to_file++;
// }else{
// if (write_particles_to_file<2){
// write_particles_to_file++;
// }
// }
/*
ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",
scan.header.stamp.toSec(),
gmap_pose.x,
gmap_pose.y,
gmap_pose.theta);
*/
return gsp_->processScan(reading);
}
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;
}
bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose)
{
if(!getOdomPose(karto_pose, scan->header.stamp))
return false;
// Create a vector of doubles for karto
std::vector<kt_double> readings;
if (lasers_inverted_[scan->header.frame_id]) {
for(std::vector<float>::const_reverse_iterator it = scan->ranges.rbegin();
it != scan->ranges.rend();
++it)
{
readings.push_back(*it);
}
} else {
for(std::vector<float>::const_iterator it = scan->ranges.begin();
it != scan->ranges.end();
++it)
{
readings.push_back(*it);
}
}
// create localized range scan
karto::LocalizedRangeScan* range_scan =
new karto::LocalizedRangeScan(laser->GetName(), readings);
range_scan->SetOdometricPose(karto_pose);
range_scan->SetCorrectedPose(karto_pose);
// Add the localized range scan to the mapper
bool processed;
if((processed = mapper_->Process(range_scan)))
{
//std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;
karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();
// Compute the map->odom transform
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (tf::Transform(tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(),
scan->header.stamp, base_frame_),odom_to_map);
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform from base_link to odom failed\n");
odom_to_map.setIdentity();
}
map_to_odom_mutex_.lock();
map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
tf::Point( odom_to_map.getOrigin() ) ).inverse();
map_to_odom_mutex_.unlock();
// Add the localized range scan to the dataset (for memory management)
dataset_->Add(range_scan);
}
else
delete range_scan;
return processed;
}
/**
* @brief SlamKarto::addScan
* @param laser LaserRangeFinder ref base
* @param scan 传感器数据 ref laser
* @param karto_pose odom_pose 回调
* @return
*/
bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose)
{
if(!getOdomPose(karto_pose, scan->header.stamp)) // 将odom与base保持坐标系对齐． 返回 odom --> karto_pose
return false;
// Create a vector of doubles for karto
std::vector<kt_double> readings;
//readings 原始scan laser -> reading
if (lasers_inverted_[scan->header.frame_id])
{
for(std::vector<float>::const_reverse_iterator it = scan->ranges.rbegin();
it != scan->ranges.rend();
++it)
{
readings.push_back(*it); //laser range messages
}
}
else
{
for(std::vector<float>::const_iterator it = scan->ranges.begin();
it != scan->ranges.end();
++it)
{
readings.push_back(*it);
debugPrint_<<" "<< *it;
}
}
debugPrint_<<" " <<endl;
if(debug_flag_)
debugPrint_<<"reading.size() "<<readings.size()<<endl;
// create localized range scan 创建rangeScan
karto::LocalizedRangeScan* range_scan =
new karto::LocalizedRangeScan(laser->GetName(), readings);//
range_scan->SetOdometricPose(karto_pose);// Sets the odometric pose of this scan
range_scan->SetCorrectedPose(karto_pose);// Moves the scan by moving the robot pose to the given location.
if(debug_print_flag_)
debugPrint_<<"1.1 addScan-karto_pose-: "<<karto_pose.GetX()<<" "<<karto_pose.GetY()<<" "<< karto_pose.GetHeading() <<" "<<endl;
// Add the localized range scan to the mapper
bool processed; //range_scan contain reading and predict pose of robot
if((processed = mapper_->Process(range_scan))) // pose 为base相对odom的pose
{
//std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;
karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();
if(debug_print_flag_)
debugPrint_<<"1.2 addScan-corrected_pose-: "<<corrected_pose.GetX()<<" "<<corrected_pose.GetY()<<" "<< corrected_pose.GetHeading() <<" "<<endl;
// Compute the map->odom transform
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (tf::Transform( tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(),
scan->header.stamp, base_frame_),odom_to_map);
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform from base_link to odom failed\n");
odom_to_map.setIdentity();
}
map_to_odom_mutex_.lock();
map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
tf::Point( odom_to_map.getOrigin() ) ).inverse();
map_to_odom_mutex_.unlock();
//debugPrint_<< " x y z "<< odom_to_map.getOrigin().getX() <<" "<< odom_to_map.getOrigin().getY()<<" "<<odom_to_map.getOrigin().getZ()<<endl;
if(debug_print_flag_)
debugPrint_<<"1.3 addScan-odom_to_map: "<<odom_to_map.getOrigin().getX() <<" "<< odom_to_map.getOrigin().getY()<<" "<<endl;
// Add the localized range scan to the dataset (for memory management)
dataset_->Add(range_scan);
}
else
delete range_scan;
return processed;
}
