自动驾驶 c++ 人工智能 cartographer

node_main.cc

坐标系的读取通过tf_bufferautonode类是cartographer_ros接收传感器数据，并传输到cartographer里，同时还会发布map，轨迹等node_options数据传给两个地方，一个是map_builder进行slam操作，一个是node做数据处理trajectory_options传给node，使用默认topic开始轨迹

**加载配置文件 **（LoadOptions）

tube：元组 c++11: std::tie()函数可以将变量连接到一个给定的tuple上,生成一个元素类型全是引用的tuple 两个参数，NodeOptions、TrajectoryOptions LoadOptions函数的作用（step） 输入两个参数：configuration_directory 配置文件所在目录 ​ configuration_basename 配置文件的名字 获取配置文件所在目录，存入file_resolver中 读取配置文件内容到code中 根据给定的字符串，生成一个lua字典 将lua中的内容填充到NodeOptions和TrajectoryOptions（通过CreateNodeOptions函数和CreateTrajectoryOptions函数）并返回，返回的是tuple元组，存放NodeOptions和TrajectoryOptions

CreateNodeOptions函数：读取lua文件内容，将lua文件内容赋值给NodeOptions CreateTrajectoryOptions函数：接收lua字典的地址，将lua文件内容赋值给TrajectoryOptions，并返回options（同上） 首先判断是否有传感去数据输入 如何读取配置文件：在node_options.cc文件中引用ConfigurationFileResolver类 继承FileResolver类 构造函数中，configuration_files_directories_变量有两个元素，一个是launch文件读入的地址，一个是通过kconfigurationFilesDirectories变量从配置文件读取的地址（此配置文件通过编译之后自动生成，编译文件在common->config.h.cmake设置） GetFileContentOrDie将读取内容拷贝到code中，GetFullPathOrDie函数根据文件名查找是否在给定的文件夹中存在

Node类

四个大的框架：

声明需要发布的topic声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中处理之后的点云发布器进行定时器与函数绑定，定时发布数据

开始轨迹

​ 从node_main中传入数据node.StartTrajectoryWithDefaultTopics(trajectory_options);

​ 调用map_builder_bridge的AddTrajectory(expected_sensor_ids, options), 添加一个轨迹

​ 新增一个位姿估计器AddExtrapolator(trajectory_id, options) imu与里程计的融合

​ 向位姿估计器（PoseExtrapolator）里面传入数据

​ 新生成一个传感器数据采样器AddSensorSamplers(trajectory_id, options)

​ TrajectorySensorSamplers结构体，控制各个传感器的采样频率

​ FixedRatioSampler类，根据给定频率对数据进行抽样，假设给定1，每一帧数据都用，给定0.5则两帧数据用一个

​ 订阅话题与注册回调函数LaunchSubscribers(options,trajectory_id)

​ 单线雷达的话题订阅与回调函数注册，HandleLaserScanMessage（回调函数）

​ 多回声波激光雷达话题订阅与回调函数注册，HandleMultiEchoLaserScanMessage（回调函数）

​ 点云话题订阅与回调函数注册，HandlePointCloud2Message（回调函数）

​ imu话题订阅与回调函数注册，HandleImuMessage（回调函数）

​ 里程计

​ GPS

​ landmarks

​ 回调函数参数皆为（trajectory_id，sensor_id，msg），在SubscribeWithHandler函数中使用lambda表达式传入。

​ 定义一个定时器，三秒执行一次，检查设置的topic名称是否存在

​ 通过MaybeWarnAboutTopicMismatch函数检查topic名称是否存在

​ 最后一个参数为oneshot，等于true表示只是执行一次

​ 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复

传感器数据的走向

在各个传感器的回调函数中处理

里程计数据走向 首先通过脉冲函数判断是否暂停 然后数据格式转换 转换后的数据类型传入位姿推测器器（extrapolators_），进行位姿估计,ros中的原始数据（msg）传入HandleOdometryMessage（sensor_id,msg）

imu数据走向 首先通过脉冲函数判断是否暂停 然后数据格式转换 转换后的数据位姿推测器器（extrapolators_），进行位姿估计与重力方向的确定；ros中的原始数据（msg）传入HandleImuMessage(sensor_id, msg)进行imu数据处理

其他传感器数据同上，GPS数据只用原始数据直接传入handle…函数

MapBuilderBridge类

local frame 与 global frame

carographer中存在两个地图坐标系, 分别为global frame与local frame

local frame是前端的坐标系, 这个坐标系与坐标系下的机器人的坐标, 一经扫描匹配之后就不会再被改变.

后端的坐标系, 回环检测与后端位姿图优化都不会对前端的任何坐标产生作用.

global frame是后端的坐标系, 是表达被回环检测与位姿图优化所更改后的坐标系.

当优化之后, 这个坐标系下的节点坐标(包括点云的位姿, submap的位姿)会发生变化.

但坐标系本身不会发生变化

三维刚体坐标变换

在**Rigid3 **这个类中，三个构造函数，一个无参数的默认构造函数，第二个传入平移和旋转的参数并赋值，第三个传入平移和轴角对translation_ 和 rotation_赋值。

供使用函数ToRigid3d

雷达数据的处理

位置：sensor_bridge.cc

void SensorBridge::HandleLaserScanMessage(const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg)

先转成点云数据，在传入trajectory_builder_

void SensorBridge::HandleLaserScanMessage(

const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {

carto::sensor::PointCloudWithIntensities point_cloud;//定义点云数据格式，带有强度和时间，

carto::common::Time time;

std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);//转为点云数据（后面的多回声波雷达和点云数据处理都调用了这个函数，使用的函数重载）

HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);

}

看看point_cloud的数据结构

struct PointCloudWithIntensities {

TimedPointCloud points;

std::vector intensities;

};

using TimedPointCloud = std::vector;

struct TimedRangefinderPoint {

Eigen::Vector3f position;

float time; // 相对点云最后一个点的时间, 最后一个点的时间为0, 其他点的时间都为负的

};

ToPointCloudWithIntensities(*msg)函数

进入ToPointCloudWithIntensities(*msg)函数

// 由ros格式的LaserScan转成carto格式的PointCloudWithIntensities

std::tuple<::cartographer::sensor::PointCloudWithIntensities,

::cartographer::common::Time>

ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {

return LaserScanToPointCloudWithIntensities(msg);

}

将ros下的激光雷达数据转成carto格式的点云数据，主要使用的就是LaserScanToPointCloudWithIntensities(msg)这个函数（多回声波和单线使用的都是调用这个同一个函数）

// 将LaserScan与MultiEchoLaserScan转成carto格式的点云数据

template

std::tuple

LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {

CHECK_GE(msg.range_min, 0.f);

CHECK_GE(msg.range_max, msg.range_min);

if (msg.angle_increment > 0.f) {

CHECK_GT(msg.angle_max, msg.angle_min);

} else {

CHECK_GT(msg.angle_min, msg.angle_max);

}

PointCloudWithIntensities point_cloud;//接着是定义了这个变量，一直向这个变量里面填充东西。最后是把point_cloud带着时间戳给返回出去。

float angle = msg.angle_min;

for (size_t i = 0; i < msg.ranges.size(); ++i) {

// c++11: 使用auto可以适应不同的数据类型

const auto& echoes = msg.ranges[i];

if (HasEcho(echoes)) {

const float first_echo = GetFirstEcho(echoes);

// 满足范围才进行使用

if (msg.range_min <= first_echo && first_echo <= msg.range_max) {

const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());//定义一个旋转向量rotation，以Z轴为旋转轴，angle为旋转角度得到的旋转向量。

const cartographer::sensor::TimedRangefinderPoint point{

rotation * (first_echo * Eigen::Vector3f::UnitX()), // position

i * msg.time_increment}; // time

// 保存点云坐标与时间信息

point_cloud.points.push_back(point);

// 如果存在强度信息

if (msg.intensities.size() > 0) {

CHECK_EQ(msg.intensities.size(), msg.ranges.size());

// 使用auto可以适应不同的数据类型

const auto& echo_intensities = msg.intensities[i];

CHECK(HasEcho(echo_intensities));

point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));

} else {

point_cloud.intensities.push_back(0.f);

}

}

}

angle += msg.angle_increment;

}

//从msg中获得时间戳

::cartographer::common::Time timestamp = FromRos(msg.header.stamp);//获得一帧激光的总时长

if (!point_cloud.points.empty()) {

const double duration = point_cloud.points.back().time;

// 以点云最后一个点的时间为点云的时间戳

timestamp += cartographer::common::FromSeconds(duration);//第一个点的时间戳+ 总时长等于最后一个点的时间戳，也就是cartographer的时间戳

// 让点云的时间变成相对值, 最后一个点的时间为0

for (auto& point : point_cloud.points) {

point.time -= duration;

}

}

return std::make_tuple(point_cloud, timestamp);

}

}

首先函数开头使用的模板参数，便无需函数重载，使用函数重载代码量大，重复率高。 LaserScan与MultiEchoLaserScan的数据类型

$ rosmsg show sensor_msgs/LaserScan

std_msgs/Header header

uint32 seq

time stamp

string frame_id

float32 angle_min

float32 angle_max

float32 angle_increment

float32 time_increment

float32 scan_time

float32 range_min

float32 range_max

float32[] ranges

float32[] intensities

$ rosmsg show sensor_msgs/MultiEchoLaserScan

std_msgs/Header header

uint32 seq

time stamp

string frame_id

float32 angle_min

float32 angle_max

float32 angle_increment

float32 time_increment

float32 scan_time

float32 range_min

float32 range_max

sensor_msgs/LaserEcho[] ranges

float32[] echoes

sensor_msgs/LaserEcho[] intensities

float32[] echoes

单线雷达和多回声波雷达数据类型的区别在于 ranges 和 intensities数组类型的不同，单线激光雷达的ranges和intensities数据类型是float32[]类型，多回声波是sensor_msgs/LaserEcho[]类型，其中还包括了float32[] echoes

根据两种不同的数据类型，进行判断，分别处理

if (HasEcho(echoes))//判断是否有数据

HasEcho(echoes)使用函数重载，以适用两种不同的数据，函数定义如下：

// For sensor_msgs::LaserScan.

bool HasEcho(float) { return true; }

// For sensor_msgs::MultiEchoLaserScan.

bool HasEcho(const sensor_msgs::LaserEcho& echo) {

return !echo.echoes.empty();

}

const float first_echo = GetFirstEcho(echoes);//获取第一个数据，用于判断是否在设置的扫描范围之内

GetFirstEcho(echoes)也是有使用的函数重载，函数定义如下：

// 通过函数重载, 使得函数可以同时适用LaserScan与LaserEcho

float GetFirstEcho(float range) { return range; }

float GetFirstEcho(const sensor_msgs::LaserEcho& echo) {

return echo.echoes[0];

}

进入数据处理过程

if (msg.range_min <= first_echo && first_echo <= msg.range_max) {

const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());//定义一个旋转向量rotation，以Z轴为旋转轴，angle为旋转角度得到的旋转向量。

const cartographer::sensor::TimedRangefinderPoint point{

rotation * (first_echo * Eigen::Vector3f::UnitX()), // position，旋转乘以一个距离值，得到位置

i * msg.time_increment}; // time，i乘以时间增量

// 保存点云坐标与时间信息

point_cloud.points.push_back(point);//point，有点的距离，时间（和强度值）

// 如果存在强度信息

if (msg.intensities.size() > 0) {

CHECK_EQ(msg.intensities.size(), msg.ranges.size());

// 使用auto可以适应不同的数据类型

const auto& echo_intensities = msg.intensities[i];

CHECK(HasEcho(echo_intensities));

point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));

} else {

point_cloud.intensities.push_back(0.f);

}

}

}

angle += msg.angle_increment;//如果距离值不满足范围，角度加上一个增量，继续判断下一个值

//从msg中获得时间戳

::cartographer::common::Time timestamp = FromRos(msg.header.stamp);//获得一帧激光的总时长

if (!point_cloud.points.empty()) {

const double duration = point_cloud.points.back().time;//duration 点云最后一个点的时间，也是一帧点云的总时长

// 以点云最后一个点的时间为点云的时间戳

timestamp += cartographer::common::FromSeconds(duration);//第一个点的时间戳+ 总时长等于最后一个点的时间戳，也就是cartographer的时间戳

// 让点云的时间变成相对值, 最后一个点的时间为0

for (auto& point : point_cloud.points) {

point.time -= duration;

}

}

return std::make_tuple(point_cloud, timestamp);

}

这一段代码通过模板参数，满足两种不同的数据格式的操作，使用了两个重载函数，完成了不同数据类型的判断，使用auto可以自适用不同的数据格式。

point_cloud2数据处理

ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg)点云数据重载

主要分为以下四种情况处理（参考源码）

有强度，有时间：直接将强度和时间赋值有强度，无时间：时间赋值0，强度赋值无强度，有时间：时间赋值，强度赋值1无强度，无时间：时间赋值0，强度赋值1

HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud)函数 HandleLaserScan

void SensorBridge::HandleLaserScan(

const std::string& sensor_id, const carto::common::Time time,

const std::string& frame_id,

const carto::sensor::PointCloudWithIntensities& points) {

if (points.points.empty()) {

return;

}

// CHECK_LE: 小于等于

CHECK_LE(points.points.back().time, 0.f);

// TODO(gaschler): Use per-point time instead of subdivisions.

// tag: 参数num_subdivisions_per_laser_scan_，在MapBuild_Bridge中由lua文件传入 // 意为一帧雷达数据被分成几次处理, 一般将这个参数设置为1 for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { const size_t start_index = points.points.size() * i / num_subdivisions_per_laser_scan_; const size_t end_index = points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;

// 生成分段的点云

carto::sensor::TimedPointCloud subdivision(

points.points.begin() + start_index, points.points.begin() + end_index);

if (start_index == end_index) {

continue;

}

const double time_to_subdivision_end = subdivision.back().time;

// `subdivision_time` is the end of the measurement so sensor::Collator will

// send all other sensor data first.

const carto::common::Time subdivision_time =

time + carto::common::FromSeconds(time_to_subdivision_end);

auto it = sensor_to_previous_subdivision_time_.find(sensor_id);

if (it != sensor_to_previous_subdivision_time_.end() &&

// 上一段点云的时间不应该大于等于这一段点云的时间

it->second >= subdivision_time) {

LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "

<< sensor_id << " because previous subdivision time "

<< it->second << " is not before current subdivision time "

<< subdivision_time;

continue;

}

// 更新对应sensor_id的时间戳

sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;

// 检查点云的时间

for (auto& point : subdivision) {

point.time -= time_to_subdivision_end;

}

CHECK_EQ(subdivision.back().time, 0.f);

// 将分段后的点云 subdivision 传入 trajectory_builder_

HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);

} // for }

其中num_subdivisions_per_laser_scan_参数，在MapBuild_Bridge中由lua文件传入，意为一帧雷达数据被分成几次处理, 一般将这个参数设置为1

for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {

const size_t start_index =

points.points.size() * i / num_subdivisions_per_laser_scan_;

const size_t end_index =

points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;

for循环计算点云开始到结束的索引，若参数num_subdivisions_per_laser_scan_值等于1，结束的索引就是点云大小，不等于1，点云大小乘以（i+1）/num_subdivisions_per_laser_scan_

假设num_subdivisions_per_laser_scan_=2，点云数据量为100，那么点云就会被分成0-50和50-100。

HandleRangefinder

雷达相关的数据最终的处理函数 先将数据转到tracking坐标系下,再调用trajectory_builder_的AddSensorData进行数据的处理

需要传入的数据，数据的话题sensor_id，点云的时间戳time，点云的frameframe_id，雷达坐标系下的TimedPointCloud格式的点云ranges

void SensorBridge::HandleRangefinder(

const std::string& sensor_id, const carto::common::Time time,

const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {

if (!ranges.empty()) {

CHECK_LE(ranges.back().time, 0.f);

}

const auto sensor_to_tracking =

tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));

// 以 tracking 到 sensor_frame 的坐标变换为TimedPointCloudData 的 origin

// 将点云的坐标转成 tracking 坐标系下的坐标, 再传入trajectory_builder_

if (sensor_to_tracking != nullptr) {

trajectory_builder_->AddSensorData(

sensor_id, carto::sensor::TimedPointCloudData{

time,

sensor_to_tracking->translation().cast(),

// 将点云从雷达坐标系下转到tracking_frame坐标系系下

carto::sensor::TransformTimedPointCloud(

ranges, sensor_to_tracking->cast())} ); // 强度始终为空

}

}

TransFromTimedPointCloud函数，返回值是坐标变换后的点云

输入值为：点云数据point_cloud，旋转变换矩阵transform,

TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,

const transform::Rigid3f& transform) {

TimedPointCloud result;

result.reserve(point_cloud.size());

for (const TimedRangefinderPoint& point : point_cloud) {

result.push_back(transform * point);

}

return result;

}

主要的代码就是在for循环中，变换矩阵乘以点云坐标result.push_back(transform * point);

Cartographer_ros中的传感器数据的传递过程总结

里程计与IMU数据的走向

里程计数据从Node类的回调函数进来, 有两个走向, 一个是Node类中的位姿估计器, 一个是

SensorBridge::HandleOdometryMessage, 再从SensorBridge传递给cartographer.

IMU数据的走向同理.

GPS与Landmark数据的走向

GPS数据从Node类的回调函数进来只有1个走向, 就是传入到SensorBridge::HandleNavSatFixMessage()函数中,

再从SensorBridge传递给cartographer.

Landmark数据的走向同理.

雷达数据的走向

存在3种雷达数据, 一种是单线点云, 一种是多回声波点云, 一种是多线雷达点云, 都是直接传入到SensorBridge中.

单线点云与多回声波雷达点云都是先经过ToPointCloudWithIntensities()函数, 处理成carto格式的点云, 然后在传

入HandleLaserScan()函数中.

根据参数配置, 将点云是否分段, 将分段的点云传入到HandleRangefinder()函数中.

多线点云是先经过ToPointCloudWithIntensities()函数, 处理成carto格式的点云, 然后在传入HandleRangefinder()

函数中.

HandleRangefinder() 函数将点云点云从雷达坐标系下转到tracking_frame坐标系系下, 并转成

数据的走向同理.

GPS与Landmark数据的走向

GPS数据从Node类的回调函数进来只有1个走向, 就是传入到SensorBridge::HandleNavSatFixMessage()函数中,

再从SensorBridge传递给cartographer.

Landmark数据的走向同理.

雷达数据的走向

存在3种雷达数据, 一种是单线点云, 一种是多回声波点云, 一种是多线雷达点云, 都是直接传入到SensorBridge中.

单线点云与多回声波雷达点云都是先经过ToPointCloudWithIntensities()函数, 处理成carto格式的点云, 然后在传

入HandleLaserScan()函数中.

根据参数配置, 将点云是否分段, 将分段的点云传入到HandleRangefinder()函数中.

多线点云是先经过ToPointCloudWithIntensities()函数, 处理成carto格式的点云, 然后在传入HandleRangefinder()

函数中.

HandleRangefinder() 函数将点云点云从雷达坐标系下转到tracking_frame坐标系系下, 并转成

TimedPointCloudData格式的点云, 然后才传入到cartographer中

好文推荐

 您阅读本篇文章共花了：