node_main.cc
- 坐标系的读取通过tf_buffer
- auto
- node类是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<float> intensities;
};
using TimedPointCloud = std::vector<TimedRangefinderPoint>;
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 <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
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()), // positioni * 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 headeruint32 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 headeruint32 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的时间戳// 让点云的时间变成相对值, 最后一个点的时间为0for (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<float>(),// 将点云从雷达坐标系下转到tracking_frame坐标系系下carto::sensor::TransformTimedPointCloud(ranges, sensor_to_tracking->cast<float>())} ); // 强度始终为空}
}
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中