InitMapper () initializes the map
Function are
The first frame of laser data is received and initialized.
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(a); 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(a);double angle_center = (scan.angle_min + scan.angle_max)/2;
if (up.z(a) >0)
{
do_reverse_range_ = scan.angle_min > scan.angle_max;
centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0.0,angle_center),
tf::Vector3(0.0.0)), ros::Time::now(), laser_frame_);
ROS_INFO("Laser is mounted upwards.");
}
else
{
do_reverse_range_ = scan.angle_min < scan.angle_max;
centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
tf::Vector3(0.0.0)), ros::Time::now(), laser_frame_);
ROS_INFO("Laser is mounted upside down.");
}
// Compute the angles of the laser from -x to x, basically symmetric and in increasing order
laser_angles_.resize(scan.ranges.size());
// Make sure angles are started so that they are centered
double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
for(unsigned int i=0; i<scan.ranges.size(a); ++i) { laser_angles_[i]=theta; theta += std::fabs(scan.angle_increment);
}
ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
scan.angle_increment);
ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
laser_angles_.back(), std::fabs(scan.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(scan.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_);
gsp_->setminimumScore(minimum_score_);
// Call the sampling function once to set the seed.
GMapping::sampleGaussian(1,seed_);
ROS_INFO("Initialization complete");
return true;
}
Copy the codeThe function framework
- Use the listener TF_ to obtain laser_pose of the laser relative to the base
- Create a point up(base_frame (0,0, lase_pose. Z +1)) 1m above the laser and convert it to the coordinates relative to the laser
- If the z-axis coordinate of up differs from 1, the laser is not installed horizontally. The initialization fails
- Get the laser center position and assign the original laser data to laser_angles (vector);
- Initialize the laser sensor model object RangeSensor and OdometrySensor
- Use getOdomPose to set the initial laser pose, or zero(0,0,0) if it fails.
- Set the matching parameters of the processor, such as the update distance, update frequency, sampling range, and sampling procedure
- Call the sample once function to set the random seed
Function analysis
-
First we see that there is a tf function tf_. TransformPose (base_frame_, ident, laser_pose); This function transfers data from frame A to frame B.
The function is defined as:
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
Copy the codeTarget_frame is the pose on which frame you want to convert the source pose to. Laser_pose (radar coordinate) is transferred to base_frame_(base coordinate) so the traget_frame of this function is base_frame_.
Stamped_in is the source pose, and stamped_out is the target data, which is the transformed data. Note that the source frame_id is not required for conversion because it is already included in stamped_in. In other words, an implicit use condition of this function is that stamped_in must specify which frame the source pose belongs to.
- This function creates a 1m high point above the radar position and converts it to a Lidar frame.
First look at the Stamped class tf::Stamped, which contains several basic data structures in addition to Transform:
template <typename T> // Template structure can be tf::Pose tf:Point and other basic structures
class Stamped : public T{
public:
ros::Time stamp_; // Record the time
std::string frame_id_; //ID
Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION") {};//Default constructor used only for preallocation
Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id);
void setData(const T& input);
};
Copy the codeThis point is first initialized and then transferred to laser_frame_(LiDAR coordinate system).
- Gmapping has no roll and pitch angles. Therefore, you need to check whether the sensor is normal
- Check whether the sensor is too high or too low
- Calculate whether the laser Angle from -x to x is symmetric and increasing sequentially.
- Make sure you start at an increasing Angle
-
Gets the laser center position and assigns the original laser data to Laser_angles
-
Initialize the laser sensor object RangeSensor and OdometrySensor
And then we come to this:
gsp_laser_ = new GMapping::RangeSensor("FLASER",
gsp_laser_beam_count_,
fabs(scan.angle_increment),
gmap_pose,
0.0,
maxRange_);
Copy the codeThis class defines a laser sensor object (explained separately below).
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
Copy the codeThen we define an odometer object (see below).
- Use getOdomPose to set the initial laser pose, or zero(0,0,0) if it fails.
The inside function getOdomPose() gets the odometer pose.
- Set the matching parameters of the processor, such as the update distance, update frequency, sampling range, and sampling procedure
Use the object GSP_ of the Class GridSlamProcessor class for processing.
- Call the sample once function to set the random seed
Fill in the pit
In the function introduction to dig a lot of holes, some classes used in the function, the use of the function are not introduced in detail, this summary will be filled in holes.
class RangeSensor
class RangeSensor: public Sensor{
friend class Configuration;
friend class CarmenConfiguration;
friend class CarmenWrapper;
public:
struct Beam{
OrientedPoint pose; //pose relative to the center of the sensor
double span; //spam=0 indicates a line-like beam
double maxRange; //maximum range of the sensor
double s,c; //sinus and cosinus of the beam (optimization);
};
RangeSensor(std::string name);
RangeSensor(std::string name, unsigned int beams, double res, const OrientedPoint& position=OrientedPoint(0.0.0), double span=0.double maxrange=89.0);
inline const std::vector<Beam>& beams(a) const {returnm_beams; }inline std::vector<Beam>& beams(a) {returnm_beams; }inline OrientedPoint getPose(a) const {returnm_pose; }void updateBeamsLookup(a);
bool newFormat;
protected:
OrientedPoint m_pose;
std::vector<Beam> m_beams;// Place the laser distance
};
Copy the codeThis class declares a laser Sensor object, which inherits from the Sensor class Sensor. This class also does not include a Sensor name variable. The friends of this class are Configuration, CarmenConfiguration, and CarmenWrapper. Here’s a quick explanation of friend classes.
The friend keyword in C++ does exactly that: it indicates in a class that other classes (or functions) have direct access to the private and protected members of that class.
The public member of this class defines a Beam structure. The pose variable M_pose and the distance m_beams where the laser is placed are also defined.
class OdometrySensor
Class OdometrySensor doesn’t have anything in it, it just defines a m_Ideal which is not clear for now.
GetOdomPose () gets the odometer pose
bool SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
{
// Get the pose of the centered laser at the right time
centered_laser_pose_.stamp_ = t;
// Get the laser's pose that is centered
tf::Stamped<tf::Transform> odom_pose;
try
{
tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
double yaw = tf::getYaw(odom_pose.getRotation());
gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
yaw);
return true;
}
Copy the codeThis function inputs a timestamp and returns the corresponding odometer pose. Note: Transform centered_LASer_pose (liDAR pose) to the odom_frame_ coordinate system the current odometer pose. In the next chapter, we’ll take a closer look at the addScan() function, which is also a core part of gmapping.