RangeReading carmen2reading(const carmen_robot_laser_message& msg){
//either front laser or rear laser
double dth=msg.laser_pose.theta-msg.robot_pose.theta;
dth=atan2(sin(dth), cos(dth));
if (msg.laser_pose.theta==msg.robot_pose.theta && !m_frontLaser){
double res=0;
res = msg.config.angular_resolution;
assert(res>0);
string sensorName="FLASER";
OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
OrientedPoint dp=absoluteDifference(lpose, rpose);
m_frontLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0,
msg.config.maximum_range);
m_frontLaser->updateBeamsLookup();
m_sensorMap.insert(make_pair(sensorName, m_frontLaser));
// cout << __PRETTY_FUNCTION__
// << ": " << sensorName <<" configured."
// << " Readings " << m_frontLaser->beams().size()
// << " Resolution " << res << endl;
}
if (msg.laser_pose.theta!=msg.robot_pose.theta && !m_rearLaser){
double res=0;
res = msg.config.angular_resolution;
assert(res>0);
OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
OrientedPoint dp=absoluteDifference(lpose, rpose);
string sensorName="RLASER";
m_rearLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0,
msg.config.maximum_range);
m_rearLaser->updateBeamsLookup();
m_sensorMap.insert(make_pair(sensorName, m_rearLaser));
// cout << __PRETTY_FUNCTION__
// << ": " << sensorName <<" configured."
// << " Readings " << m_rearLaser->beams().size()
// << " Resolution " << res << endl;
}
const RangeSensor * rs=(msg.laser_pose.theta==msg.robot_pose.theta)?m_frontLaser:m_rearLaser;
RangeReading reading(rs, msg.timestamp);
reading.resize(rs->beams().size());
for (unsigned int i=0; i< (unsigned int)msg.num_readings; i++){
reading[i]=(double)msg.range[i];
}
reading.setPose(OrientedPoint(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta));
return 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::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;
}