1 cartographer_node总结：
约有代码7711行，6000行以上是C++代码。cartographer_ros package中包含多個node，每个node都有main函数入口。launch文件中cartographer_node的入口函数函数是node_main.cc，该文件路径为：/src/cartographer_ros/cartographer_ros/cartographer_ros下。
node_main.cc跳转node.cc，再跳转到map_builder_bridge.cc，最后跳转到cartographer包下的builder_bridge.cc具体实现
cartographer启动以后，做了如下的工作：
- 注册并发布了6个Topic, 并为6个Topic分别设置了定时器函数，在定时器函数中定期向Topic上广播数据：
-
Topic 1: kSubmapListTopic: 广播构建出来的submap的list
发布数据的函数：Node::PublishSubmapList
调用函数：map_builder_bridge_.GetSubmapList(); -
Topic 2: kTrajectoryNodeListTopic：发布trajectory
发布数据的函数：Node::PublishTrajectoryNodeList Node::PublishTrajectoryStates：
调用函数：map_builder_bridge_.GetTrajectoryNodeList(); -
Topic3: kLandmarkPoseListTopic
发布数据的函数：Node::PublishLandmarkPosesList
调用的函数：map_builder_bridge_.GetLandmarkPoseList()；
- Topic4: kConstraintListTopic
发布数据的函数：Node::PublishConstraintList
调用的函数：map_builder_bridge_.GetConstraintList()；
5）Topic5: kTrackedPoseTopic 和local slam相关的
发布数据的函数：Node::PublishLocalTrajectoryData
调用的函数： extrapolator.AddPose(trajectory_data.local_slam_data->time,
trajectory_data.local_slam_data->local_pose);
6）Topic5: kScanMatchedPointCloudTopic
发布数据的函数：暂时没找到
调用的函数：
- 发布了7个Service，并为7个Service分别设置了句柄函数，而句柄函数也是通过调用map_builder_bridge_的成员函数来处理的。
service_servers_.push_back(node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kWriteStateServiceName, &Node::HandleWriteState, this));
service_servers_.push_back(node_handle_.advertiseService(
kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
service_servers_.push_back(node_handle_.advertiseService(
kReadMetricsServiceName, &Node::HandleReadMetrics, this));
cartographer_ros这个节点启动后所有东西都交给了map_builder_bridge_去处理。map_builder_bridge_是cartographer_node和cartographer交互的桥梁。
2 MapBuilderBridge
cartographer_node这个节点的所有操作，都交给了MapBuilderBridge的一个对象map_builder_bridge_来完成。MapBuilderBridge这个类里我们依然看不到任何关于算法的影子，它也只是一层封装。
MapBuilderBridge定义在/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h中，实现由…/http://map_builder_bridge.cc完成。
类的定义：
namespace cartographer_ros {
class MapBuilderBridge {
public:
struct LocalTrajectoryData {
// Contains the trajectory data received from local SLAM, after
// it had processed accumulated 'range_data_in_local' and estimated
// current 'local_pose' at 'time'.
struct LocalSlamData {
::cartographer::common::Time time;
::cartographer::transform::Rigid3d local_pose;
::cartographer::sensor::RangeData range_data_in_local;
};
std::shared_ptr<const LocalSlamData> local_slam_data;
cartographer::transform::Rigid3d local_to_map;
std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
TrajectoryOptions trajectory_options;
};
MapBuilderBridge(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* tf_buffer);
MapBuilderBridge(const MapBuilderBridge&) = delete;
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
void LoadState(const std::string& state_filename, bool load_frozen_state);
int AddTrajectory(
const std::set<
::cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options);
void FinishTrajectory(int trajectory_id);
void RunFinalOptimization();
bool SerializeState(const std::string& filename,
const bool include_unfinished_submaps);
void HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response);
void HandleTrajectoryQuery(
cartographer_ros_msgs::TrajectoryQuery::Request& request,
cartographer_ros_msgs::TrajectoryQuery::Response& response);
std::map<int /* trajectory_id */,
::cartographer::mapping::PoseGraphInterface::TrajectoryState>
GetTrajectoryStates();
cartographer_ros_msgs::SubmapList GetSubmapList();
std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
LOCKS_EXCLUDED(mutex_);
visualization_msgs::MarkerArray GetTrajectoryNodeList();
visualization_msgs::MarkerArray GetLandmarkPosesList();
visualization_msgs::MarkerArray GetConstraintList();
SensorBridge* sensor_bridge(int trajectory_id);
private:
void OnLocalSlamResult(const int trajectory_id,
const ::cartographer::common::Time time,
const ::cartographer::transform::Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local)
LOCKS_EXCLUDED(mutex_);
absl::Mutex mutex_;
const NodeOptions node_options_;
std::unordered_map<int,
std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>
local_slam_data_ GUARDED_BY(mutex_);
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
tf2_ros::Buffer* const tf_buffer_;
std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_;
// These are keyed with 'trajectory_id'.
std::unordered_map<int, TrajectoryOptions> trajectory_options_;
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
std::unordered_map<int, size_t> trajectory_to_highest_marker_id_;
};
} // namespace cartographer_ros
类的实现
2.1 跟显示相关
visualization_msgs::Marker
::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) {
::std_msgs::ColorRGBA result;
result.r = color[0];
result.g = color[1];
result.b = color[2];
result.a = 1.f;
return result;
}
visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id,
const std::string& frame_id) {
visualization_msgs::Marker marker;
marker.ns = absl::StrCat("Trajectory ", trajectory_id);
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.header.stamp = ::ros::Time::now();
marker.header.frame_id = frame_id;
marker.color = ToMessage(cartographer::io::GetColor(trajectory_id));
marker.scale.x = kTrajectoryLineStripMarkerScale;
marker.pose.orientation.w = 1.;
marker.pose.position.z = 0.05;
return marker;
}
void PushAndResetLineMarker(visualization_msgs::Marker* marker,
std::vector<visualization_msgs::Marker>* markers) {
markers->push_back(*marker);
++marker->id;
marker->points.clear();
}
2.2 与LandMark相关的。
.lua的use_landmarks = false，这块基本没用到
2.3. 构造函数
构造函数里只是做了一下实例化赋值：
MapBuilderBridge::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) {}
2.4. LoadState函数：
调用了map_builder_的成员函数LoadState来加载一个.pbstream文件。map_builder_是接口MapBuilderInterface的实例化对象，而根据是2d还是3d情况，其具体实现会略有不同，这里我只考虑2D。
void MapBuilderBridge::LoadState(const std::string& state_filename,
bool load_frozen_state) {
// Check if suffix of the state file is ".pbstream".
const std::string suffix = ".pbstream";
CHECK_EQ(state_filename.substr(
std::max<int>(state_filename.size() - suffix.size(), 0)),
suffix)
<< "The file containing the state to be loaded must be a "
".pbstream file.";
LOG(INFO) << "Loading saved state '" << state_filename << "'...";
cartographer::io::ProtoStreamReader stream(state_filename);
map_builder_->LoadState(&stream, load_frozen_state);
}
2.5.和Trajectory相关
AddTrajectory。添加一条Trajectory，最终的实现是在cartographer::mapping的map_builder_中实现
int MapBuilderBridge::AddTrajectory(
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options) {
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options,
[this](const int trajectory_id, const ::cartographer::common::Time time,
const Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<
const ::cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>) {
OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
});
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
// Make sure there is no trajectory with 'trajectory_id' yet.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
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));
auto emplace_result =
trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;
}
下来的这三个函数最终调用调用了map_builder_的成员函数来处理：
void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";
// Make sure there is a trajectory with 'trajectory_id'.
CHECK(GetTrajectoryStates().count(trajectory_id));
map_builder_->FinishTrajectory(trajectory_id);
sensor_bridges_.erase(trajectory_id);
}
void MapBuilderBridge::RunFinalOptimization() {
LOG(INFO) << "Running final trajectory optimization...";
map_builder_->pose_graph()->RunFinalOptimization();
}
2.6. 用于rviz显示等的Trajectory/Submap获取
以下是函数相关函数的声明，定义在cpp中，不具体一一列出
void HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response);
void HandleTrajectoryQuery(
cartographer_ros_msgs::TrajectoryQuery::Request& request,
cartographer_ros_msgs::TrajectoryQuery::Response& response);
std::map<int /* trajectory_id */,
::cartographer::mapping::PoseGraphInterface::TrajectoryState>
GetTrajectoryStates();
cartographer_ros_msgs::SubmapList GetSubmapList();
std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
LOCKS_EXCLUDED(mutex_);
visualization_msgs::MarkerArray GetTrajectoryNodeList();
visualization_msgs::MarkerArray GetLandmarkPosesList();
visualization_msgs::MarkerArray GetConstraintList();
2.7. 调用sensor_bridge
返回一个SensorBridge的指针。
SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
return sensor_bridges_.at(trajectory_id).get();
}
在处理传感器的数据时，通过调用map_builder_bridge_的一个类成员sensor_bridge，进入到SensorBridge类，对传感器数据进行处理。具体的实现在SensorBridge类中，具体文件sensor_bridge.h和sensor_bridge.cpp，该文件路径为：/src/cartographer_ros/cartographer_ros/cartographer_ros下
3. sensor_bridge
MapBuilderBridge类用于cartographer_ros和cartographer之间数据类型的转换。去ROS过程中应重点关注该部分。
MapBuilderBridge中一个函数MapBuilderBridge::sensor_bridge可以返回指定trajectory_id的SensorBridge变量，然后在SensorBridge中处理相关传感器的信息。
以IMU为例，在cartographer_node中由SensorBridge::HandleImuMessage函数处理，该函数实际调用的是map_builder_bridge_->sensor_bridge->HandleImuMessage。
3.1 构造函数等
把构造函数的参数赋值给成员变量。但需要注意的是成员变量中有一个carto::mapping::TrajectoryBuilderInterface型的一个指针变量trajectory_builder_。继续跟踪代码我们可以发现，cartographer中各种消息都统一调用了这个成员类的虚函数trajectory_builder_->AddSensorData。
这个类都定义在cartographer中的mapping文件夹下。
SensorBridge::SensorBridge(
const int num_subdivisions_per_laser_scan,
const std::string& tracking_frame,
const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
: num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
trajectory_builder_(trajectory_builder) {}
3.2. 处理里程计的函数
传感器的ROS节点/Playbag广播到Topic上相关数据的Message---->cartographer_node中启动的AddTrajectory----->LaunchSubscribers()------>Node::HandleImuMessage---->sensor_bridge_ptr->HandleOdometryMessage()该处理函数实际调用是MapBuilderBridge中的一个sensor_bridge_ptr变量进行处理---->trajectory_builder_->AddSensorData 调用了TrajectoryBuilder的虚函数AddSensorData()
std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
const nav_msgs::Odometry::ConstPtr& msg) {
const carto::common::Time time = FromRos(msg->header.stamp);
const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
time, CheckNoLeadingSlash(msg->child_frame_id));
if (sensor_to_tracking == nullptr) {
return nullptr;
}
return absl::make_unique<carto::sensor::OdometryData>(
carto::sensor::OdometryData{
time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
}
void SensorBridge::HandleOdometryMessage(
const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
std::unique_ptr<carto::sensor::OdometryData> odometry_data =
ToOdometryData(msg);
if (odometry_data != nullptr) {
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
}
}
3.3. 处理IMU数据
用的是加速度和角速度
std::unique_ptr<carto::sensor::ImuData> SensorBridge::ToImuData(
const sensor_msgs::Imu::ConstPtr& msg) {
CHECK_NE(msg->linear_acceleration_covariance[0], -1)
<< "Your IMU data claims to not contain linear acceleration measurements "
"by setting linear_acceleration_covariance[0] to -1. Cartographer "
"requires this data to work. See "
"http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
CHECK_NE(msg->angular_velocity_covariance[0], -1)
<< "Your IMU data claims to not contain angular velocity measurements "
"by setting angular_velocity_covariance[0] to -1. Cartographer "
"requires this data to work. See "
"http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
const carto::common::Time time = FromRos(msg->header.stamp);
const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
time, CheckNoLeadingSlash(msg->header.frame_id));
if (sensor_to_tracking == nullptr) {
return nullptr;
}
CHECK(sensor_to_tracking->translation().norm() < 1e-5)
<< "The IMU frame must be colocated with the tracking frame. "
"Transforming linear acceleration into the tracking frame will "
"otherwise be imprecise.";
return absl::make_unique<carto::sensor::ImuData>(carto::sensor::ImuData{
time, sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
}
void SensorBridge::HandleImuMessage(const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
if (imu_data != nullptr) {
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
imu_data->angular_velocity});
}
}
3.4. 处理激光、多激光、点云数据
LaserScan 2D激光
MultiEchoLaserScan 多回波雷达
PointCloud2 3D LiDAR点云
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);
}
void SensorBridge::HandleMultiEchoLaserScanMessage(
const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::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);
}
void SensorBridge::HandlePointCloud2Message(
const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
}
const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
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(points.points.back().time, 0.f);
// TODO(gaschler): Use per-point time instead of subdivisions.
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_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);
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
}
}
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));
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::TimedPointCloudData{
time, sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformTimedPointCloud(
ranges, sensor_to_tracking->cast<float>())});
}
}
3.5. 处理Landmark和NavSatFix
省略，室内基本用不到
cartographer_ros主要来处理输入输出数据，slam的核心工作都交给了MapBuilder和TrajectoryBuilder这两个类。所以，应该可以详见，在cartographer这部分的代码中，应该是以这两个类为核心开展的。所以，下一篇文章我们将以MapBuilder和TrajectoryBuilder为核心来查看代码。
5231
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
