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 code

The function framework

  1. Use the listener TF_ to obtain laser_pose of the laser relative to the base
  2. 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
  3. If the z-axis coordinate of up differs from 1, the laser is not installed horizontally. The initialization fails
  4. Get the laser center position and assign the original laser data to laser_angles (vector);
  5. Initialize the laser sensor model object RangeSensor and OdometrySensor
  6. Use getOdomPose to set the initial laser pose, or zero(0,0,0) if it fails.
  7. Set the matching parameters of the processor, such as the update distance, update frequency, sampling range, and sampling procedure
  8. Call the sample once function to set the random seed

Function analysis

  1. 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 code

Target_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.

  1. 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 code

This point is first initialized and then transferred to laser_frame_(LiDAR coordinate system).

  1. 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

  1. Gets the laser center position and assigns the original laser data to Laser_angles

  2. 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 code

This class defines a laser sensor object (explained separately below).

 gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
Copy the code

Then we define an odometer object (see below).

  1. Use getOdomPose to set the initial laser pose, or zero(0,0,0) if it fails.

The inside function getOdomPose() gets the odometer pose.

  1. 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.

  1. 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 code

This 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 code

This 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.