Gmapping 源码阅读(3)————初始化地图

企业开发 2021-11-25 15:32:55 阅读次数: 0

initMapper()对地图进行初始化

函数作用

收到第一帧的激光数据并对该数据进行初始化。

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();

  double angle_center = (scan.angle_min + scan.angle_max)/2;

  if (up.z() > 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(); ++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;
}
复制代码

函数框架

  1. 利用监听tf_ 获取激光相对于基座的位姿laser_pose
  2. 创建一个激光上方1m的点up(base_frame下 （0，0，lase_pose.z+1））,然后转化为相对于激光的坐标
  3. 如果up的z轴坐标和1有相差，则说明激光未安装水平。则初始化失败
  4. 获取激光中心位置 ，并将原始激光数据赋值给laser_angles (类型为vector);
  5. 初始化激光传感器模型对象RangeSensor ，里程计模型对象OdometrySensor
  6. 利用getOdomPose 设置初始激光位姿，设置失败则设为zero(0,0,0)
  7. 设置处理器的匹配参数，更新距离、更新频率、采样范围、采样步骤等参数
  8. 调用采样一次函数，设置随机种子

函数解析

  1. 首先我们看到有一个tf函数 tf_.transformPose(base_frame_, ident, laser_pose);该函数的作用是将A坐标系中的数据转到B坐标系上。

    该函数的定义为:

void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const 
复制代码

target_frame就是你要把源pose转换成哪个frame上的pose。这里需要将laser_pose(雷达坐标系)转到base_frame_(基坐标系)上所以该函数的的traget_frame为base_frame_。

stamped_in就是源pose，而stamped_out就是目标数据了，也就是转换完成的数据。需要注意的是，从参数上来看，转换时是不需要指定源frame_id的，这是因为它已经包含在了stamped_in中，换句话说，就是这个函数一个隐含的使用条件是，stamped_in中必须指明源pose属于哪个frame。

  1. 该函数创建一个1m高的点在雷达位置的上方并且将它转换到激光雷达帧。

首先我们看一下tf::Stamped这个类,是一种包含了除了Transform的其他几种基本的数据结构的一种数据结构:

template <typename T>   //模版结构可以是tf::Pose tf:Point 这些基本的结构
class Stamped : public T{
public:
  ros::Time stamp_;    //记录时间
  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);
};
复制代码

首先初始化这个点，然后将这个点转到laser_frame_(激光雷达坐标系)。

  1. gmapping没有横滚角(roll)和俯仰角(pitch)。所以需要检查传感器是否正常
  • 检查传感器是否过高或过低
  • 计算激光从-x到x的角度是否对称且顺序递增。
  • 确保开始的角度是递增的

  1. 获取激光中心位置 ，并将原始激光数据赋值给laser_angles

  2. 初始化激光传感器对象RangeSensor和里程计传感器对象OdometrySensor

然后我们来到这一句:

  gsp_laser_ = new GMapping::RangeSensor("FLASER",
                                         gsp_laser_beam_count_,
                                         fabs(scan.angle_increment),
                                         gmap_pose,
                                         0.0,
                                         maxRange_);
复制代码

这个类定义了一个激光传感器对象(后面单独讲)。

扫描二维码关注公众号，回复： 13345882 查看本文章 
 gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
复制代码

然后又定义一个里程计的对象(后面单独讲)。

  1. 利用getOdomPose 设置初始激光位姿，设置失败则设为zero(0,0,0)

里面函数getOdomPose()获取里程计位姿。

  1. 设置处理器的匹配参数，更新距离、更新频率、采样范围、采样步骤等参数

使用 class GridSlamProcessor 类的对象gsp_进行处理。

  1. 调用采样一次函数，设置随机种子

填坑

在函数介绍中挖了许多坑，一些在函数中使用的类，使用的函数都没有详解介绍，本小结将一一填坑。

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() const {return m_beams;}
		inline std::vector<Beam>& beams() {return m_beams;}
		inline OrientedPoint getPose() const {return m_pose;}
		void updateBeamsLookup();
		bool newFormat;
	protected:
		OrientedPoint m_pose;
		std::vector<Beam> m_beams;//放置激光的距离
};
复制代码

该类声明了一个激光传感器对象，该类继承了传感器类Sensor该类里也没什么，主要就一个传感器名的变量。 该类的友元类有，Configuration、CarmenConfiguration、CarmenWrapper。这里简单说明一下友元类。

C++中的friend关键字其实做这样的事情：在一个类中指明其他的类（或者）函数能够直接访问该类中的private和protected成员。

该类的public成员定义了一个Beam结构体。同时还定义了位姿的变量m_pose和放置激光的距离m_beams。

class OdometrySensor

class OdometrySensor中也没啥东西，主要就定义了一个m_ideal它的作用暂时不太清除。

getOdomPose()获取里程计位姿
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;
}
复制代码

该函数输入时间戳，返回对应的里程计位姿。 注意: 将centered_laser_pose(激光雷达位姿)变换到odom_frame_坐标系上即当前里程计位姿。 下一章，我们详细看一下addScan()函数，这个函数也是gmapping中比较核心的部分。

猜你喜欢

转载自juejin.im/post/7034394375094272007
今日推荐
《美国对全球网络空间安全与发展的威胁和破坏》报告发布
火速冲上 GitHub 热榜 —— 开源编程语言、框架哪有这么可爱？
北京人形机器人创新中心发布全球首个纯电驱拟人奔跑的全尺寸人形机器人“天工”
LFOSSA 源来如此公开课 | 掌握云原生未来：CNCF 认证全面攻略与备考秘籍
国产云输入法——仅华为无云端数据上传安全问题
周排行
Python环境安装与基础语法（1）——计算机基础知识
IMU预积分
ADAS中的LDW、FCW、BSD、LCA、ACC、AEB、APA、DMS代表的含义
B站笔试两道题
skyeye arm 硬件虚拟机环境的搭建
Web前端静态页面示例
数组-合并排序数组 II-简单
springcloud之版本问题启动报错
面向对象-------------匿名对象(六)
输入URL到页面呈现中间发生了什么？
每日归档
更多
2024-04-30(1)
2024-04-29(40)
2024-04-28(0)
2024-04-27(56)
2024-04-26(39)
2024-04-25(22)
2024-04-24(36)
2024-04-23(26)
2024-04-22(39)
2024-04-21(0)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022