gmapping 实现过程和数据走向

方小生–

于 2024-09-05 21:59:00 发布

阅读量287

点赞数 3

分类专栏： gmapping 文章标签： gmapping

本文链接：https://blog.csdn.net/qq2650326396/article/details/141367593

版权

gmapping 专栏收录该内容

2 篇文章 1 订阅

订阅专栏

全局地图坐标系`gmap_pose`数据处理过程

初始化Map位姿

初始化地图坐标map：map是不会变化的，是gmapping 程序启动的位置姿态就是地图的原地

  // 地图原点设置为激光中心位置 启动程序时，设置原点
  GMapping::OrientedPoint gmap_pose(0, 0, 0);

激光初始化里程计的位姿

  // 获取初始姿态
  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);
  }

获取当前激光雷达时间下的odom->lase的tf来得到里程计的位姿
通过tf变换来计算里程计的位姿

/**
 * @brief 尝试获取指定时间下的机器人里程计位置
 * 
 * @param gmap_pose 输出参数，用于存储计算出的里程计位置
 * @param t 指定的时间点
 * 
 * @return 返回是否成功获取里程计位置
 */
bool SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
{
  // 获取中心激光雷达在指定时间的位置
  centered_laser_pose_.stamp_ = t;

  // 使用TF变换获取激光雷达中心位置对应的里程计坐标系下的位置
  tf::Stamped<tf::Transform> odom_pose;
  try
  {
    tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
  }
  catch(tf::TransformException e)
  {
    ROS_WARN("计算里程计位置失败，跳过扫描 (%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;
}

传递位姿数据`setPose`—>`getPose()`

后面通过getPose来读取gmap_pose的位姿

  // 设置激光雷达扫描的姿态
  reading.setPose(gmap_pose);

`gmap_pose`数据保存和传递

// 防止多次定义
#ifndef ODOMETRYREADING_H
#define ODOMETRYREADING_H

// 引入基本的传感器读数类和点类
#include <string.h>
#include <sensor/sensor_base/sensorreading.h>
#include <utils/point.h>
// 引入里程计传感器类
#include "odometrysensor.h"

// 命名空间GMapping，用于地图构建相关功能
namespace GMapping{

// 里程计读数类，继承自SensorReading
class OdometryReading: public SensorReading{
	public:
		// 构造函数，初始化里程计读数
		// 参数odo: 里程计传感器对象的指针
		// 参数time: 读数的时间戳，默认为0
		OdometryReading(const OdometrySensor* odo, double time=0);
		
		// 获取当前姿态
		inline const OrientedPoint& getPose() const {return m_pose;}
		// 获取当前速度
		inline const OrientedPoint& getSpeed() const {return m_speed;}
		// 获取当前加速度
		inline const OrientedPoint& getAcceleration() const {return m_acceleration;}
		
		// 设置当前姿态
		inline void setPose(const OrientedPoint& pose) {m_pose=pose;}
		// 设置当前速度
		inline void setSpeed(const OrientedPoint& speed) {m_speed=speed;}
		// 设置当前加速度
		inline void setAcceleration(const OrientedPoint& acceleration) {m_acceleration=acceleration;}
		
	protected:
		// 当前姿态
		OrientedPoint m_pose;
		// 当前速度
		OrientedPoint m_speed;
		// 当前加速度
		OrientedPoint m_acceleration;
};

};
// 结束防止多次定义
#endif