目录
[local frame 与 global frame](#local frame 与 global frame)
Cartographer_ros中的传感器数据的传递过程总结
函数指针
#define CHECK_EQ(val1, val2) CHECK_OP(_EQ, ==, val1, val2)
#define CHECK_NE(val1, val2) CHECK_OP(_NE, !=, val1, val2)
#define CHECK_LE(val1, val2) CHECK_OP(_LE, <=, val1, val2)
#define CHECK_LT(val1, val2) CHECK_OP(_LT, < , val1, val2)
#define CHECK_GE(val1, val2) CHECK_OP(_GE, >=, val1, val2)
#define CHECK_GT(val1, val2) CHECK_OP(_GT, > , val1, val2)
int function ( int a , int b )
{
// 执行代码
}
int main ( void )
{
int ( * FP )( int , int ); // 函数指针的声明方式
FP = function ; // 第一种赋值方法
// FP = &function; // 第二种赋值方法
FP ( 1 , 2 ); // 第一种调用方法
// (*FP)(1,2); // 第二种调用方法
return 0 ;
}
class MyClass {
public :
int Fun ( int a , int b ) {
cout << "call Fun" << endl ;
return a + b ;
}
};
int main () {
MyClass * obj = new MyClass ;
int ( MyClass:: * pFun )( int , int ) = & MyClass::Fun ; // 成员函数指针的声明与初始化
( obj ->* pFun )( 1 , 2 ); // 通过对象来调用成员函数指针
delete obj ;
return 0 ;
}
lambda****表达式
订阅话题的名字对不上时会提示这样的错误信息
错误信息截图展示如下
函数对象参数 = \& ( 操作符重载函数参数 ) mutable 或 exception 声明 ( -> 返回值类型 ) { 函数
体 }
eg1
node , handler , trajectory_id , topic ( const typename MessageType::ConstPtr & msg )
{
( node ->* handler )( trajectory_id , topic , msg );
}
eg2
const Rigid3d tracking_to_local = \& {
// 是否将变换投影到平面上
if ( trajectory_data . trajectory_options . publish_frame_projected_to_2d ) {
return carto::transform::Embed3D (
carto::transform::Project2D ( tracking_to_local_3d ));
}
return tracking_to_local_3d ;
}();
传感器数据的走向与传递
传感器数据的回调函数
HandleOdometryMessage
HandleNavSatFixMessage
HandleLandmarkMessage
HandleImuMessage
HandleLaserScanMessage
HandleMultiEchoLaserScanMessage
HandlePointCloud2Message
MapBuilderBridge
local frame与global frame
carographer 中存在两个地图坐标系 , 分别为 global frame 与 local frame
local frame 是前端的坐标系 , 这个坐标系与坐标系下的机器人的坐标 , 一经扫描匹配之后就不会再被改变 .
后端的坐标系 , 回环检测与后端位姿图优化都不会对前端的任何坐标产生作用 .
global frame 是后端的坐标系 , 是表达被回环检测与位姿图优化所更改后的坐标系 .
当优化之后 , 这个坐标系下的节点坐标 ( 包括点云的位姿 , submap 的位姿 ) 会发生变化 .
但坐标系本身不会发生变化 .
雷达数据的格式转换与坐标变换
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[] echoesCartographer_ros****中的传感器数据的传递过程总结
里程计与 IMU 数据的走向
里程计数据从 Node 类的回调函数进来 , 有两个走向 , 一个是 Node 类中的位姿估计器 , 一个是
SensorBridge::HandleOdometryMessage, 再从 SensorBridge 传递给 cartographer
GPS 与 Landmark 数据的走向
GPS 数据从 Node 类的回调函数进来只有 1 个走向 , 就是传入到 SensorBridge::HandleNavSatFixMessage() 函数中 ,
再从 SensorBridge 传递给 cartographer.
Landmark 数据的走向同理 .
雷达数据的走向
存在 3 中雷达数据 , 一种是单线点云 , 一种是多线点云 , 一种是多回声波雷达点云 , 都是直接传入到 SensorBridge 中 .
单线点云与多回声波雷达点云都是先经过 ToPointCloudWithIntensities() 函数 , 处理成 carto 格式的点云 , 然后在传
入 HandleLaserScan() 函数中 .
根据参数配置 , 将点云是否分段 , 将分段的点云传入到 HandleRangefinder() 函数中 .
多线点云是先经过 ToPointCloudWithIntensities() 函数 , 处理成 carto 格式的点云 , 然后在传入 HandleRangefinder()
函数中 .
HandleRangefinder() 函数将点云点云从雷达坐标系下转到 tracking_frame 坐标系系下 , 并转成
TimedPointCloudData 格式的点云 , 然后才传入到 cartographer 中
传感器数据的分发处理
node_main.cc
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
Node node(node_options, std::move(map_builder), &tf_buffer, FLAGS_collect_metrics);Node 类的构造函数
Node::Node(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
: node_options_(node_options),
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)
{
// Step: 1 声明需要发布的topic
// Step: 2 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
// Step: 3 处理之后的点云的发布器
// Step: 4 进行定时器与函数的绑定, 定时发布数据
}MapBuilderBridge 类的构造函数
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer)
: node_options_(node_options),
map_builder_(std::move(map_builder)),
tf_buffer_(tf_buffer) {}开始轨迹时的处理
node_main.cc 的 StartTrajectoryWithDefaultTopics 函数
Node 类的 StartTrajectoryWithDefaultTopics 函数
MapBuilderBridge 类的 AddTrajectory 函数
int MapBuilderBridge::AddTrajectory() {
// Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options,
[this]() {
OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
});
// Step: 2 为这个新轨迹 添加一个SensorBridge
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
trajectory_options.num_subdivisions_per_laser_scan,
trajectory_options.tracking_frame,
node_options_.lookup_transform_timeout_sec,
tf_buffer_,
map_builder_->GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder
return trajectory_id;
}MapBuilder 类的 AddTrajectoryBuilder 函数
int MapBuilder::AddTrajectoryBuilder(
const std::set<SensorId>& expected_sensor_ids,
const proto::TrajectoryBuilderOptions& trajectory_options,
LocalSlamResultCallback local_slam_result_callback) {
// CollatedTrajectoryBuilder初始化
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
trajectory_options,
sensor_collator_.get(), // sensor::Collator
trajectory_id,
expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback, pose_graph_odometry_motion_filter)));
return trajectory_id;
}