在我们利用cartographer建图完成后用来定位时，就需要获取位姿信息，那么该如何获取后端的位姿和时间并将其作为topic信息发布出来呢？本篇文章将手把手教你如何实现。
一.明确需要的数据
（1）首先我们需要知道的是，底层cartographer主要依靠MapBuilder类来进行地图的构建、节点局部位姿和全局位姿的确定，地图的构建依赖Submap，局部位姿靠local_slam，全局位姿优化靠pose_graph，既然我们需要位姿这个数据，不妨将目光集中在pose_graph身上。
（2）pose_graph的数据储存在pose_graph_data里面，因此我们就可以将目光放在cartographer/mapping/pose_graph_data.h文件上，注意：cartographer/pose_graph文件夹下也有pose_graph_data.h文件，但是此文件包含数据量较少，不是我们所需要，先略过。
在cartographer/mapping/pose_graph_data.h文件中，我们可以发现这样一个数据结构体：
struct PoseGraphData {
// Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations.
MapById<SubmapId, InternalSubmapData> submap_data;
// Global submap poses currently used for displaying data.
MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;
// Data that are currently being shown.
MapById<NodeId, TrajectoryNode> trajectory_nodes;
// Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
landmark_nodes;
// How our various trajectories are related.
TrajectoryConnectivityState trajectory_connectivity_state;
int num_trajectory_nodes = 0;
std::map<int, InternalTrajectoryState> trajectories_state;
// Set of all initial trajectory poses.
std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;
std::vector<PoseGraphInterface::Constraint> constraints;
};
（3）这是一个所含数据较全的struct，包含了子图数据、轨迹节点数据、地标节点数据、轨迹状态、轨迹初始位姿、节点约束等数据，回顾我们的目标——找位姿，位姿是什么的位姿？那必然是轨迹上各个节点的位姿，所以我们将目光投向MapById<NodeId, TrajectoryNode> trajectory_nodes上。这是一个容器，通过轨迹的id查询得到对应轨迹数据，所以，从上面的分析可以知道，我们需要的是TrajectoryNode数据。
（4）接下来就将目光转移到TrajectoryNode上，该类定义在cartographer/mapping/trajectory_node.h中
namespace cartographer {
namespace mapping {
struct TrajectoryNodePose {
struct ConstantPoseData {
common::Time time;
transform::Rigid3d local_pose;
};
// The node pose in the global SLAM frame.
transform::Rigid3d global_pose;
absl::optional<ConstantPoseData> constant_pose_data;
};
struct TrajectoryNode {
struct Data {
common::Time time;
// Transform to approximately gravity align the tracking frame as
// determined by local SLAM.
Eigen::Quaterniond gravity_alignment;
// Used for loop closure in 2D: voxel filtered returns in the
// 'gravity_alignment' frame.
sensor::PointCloud filtered_gravity_aligned_point_cloud;
// Used for loop closure in 3D.
sensor::PointCloud high_resolution_point_cloud;
sensor::PointCloud low_resolution_point_cloud;
Eigen::VectorXf rotational_scan_matcher_histogram;
// The node pose in the local SLAM frame.
transform::Rigid3d local_pose;
};
common::Time time() const { return constant_data->time; }
// This must be a shared_ptr. If the data is used for visualization while the
// node is being trimmed, it must survive until all use finishes.
std::shared_ptr<const Data> constant_data;
// The node pose in the global SLAM frame.
transform::Rigid3d global_pose;
};
proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data);
TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto);
} // namespace mapping
} // namespace cartographer
从该文件中我们可以发现有两个结构体，一是TrajectoryNodePose，二是TrajectoryNode，通过比较我们可以发现TrajectoryNodePose其实是TrajectoryNode的简化版，因为我们只需要位姿，而不需要点云等其他信息，所以后续我们将以获取TrajectoryNodePose数据为目标。
二、书写接口函数
在明确所要获取的目标数据下，接下来我们就需要想办法设计接口函数，考虑如何实现在cartographer_ros上层拿到这个TrajectoryNodePose数据并将其作为ROS topic数据发出来。
我们知道cartographer_ros是通过map_build_bridge来搭建桥梁连通底层cartographer算法的，不知道的小伙伴可以看看我写的cartographer学习记录系列的分析（cartographer源码3D SLAM部分一、二、三）。那么我们应该如何写呢，下面我将进行先分析，后搭建的方式来教大家如何实现。
（1）分析：
首先呢，我们已经明确了TrajectoryNodePose数据作为我们的目标，反退一步，该数据是处于PoseGraphData类内的MapById<NodeId, TrajectoryNode> trajectory_nodes上，因为我们是3D SLAM系列，所以我们不妨去cartographer/mapping/internal/3d/pose_graph_3d.h下找找其本身有没有实现获取该数据的方法。这一找我们发现还真有该函数。
MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
LOCKS_EXCLUDED(mutex_);
如果没有获取该数据的方法我们就需要接着分析该类内包含的数据对象，想办法从中分离出TrajectoryNodePose数据，包括时间戳，局部位姿，全局位姿。
既然有此函数，那么我们就可以在上层搭建接口函数去调用该函数获取底层cartographer的TrajectoryNodePose数据。
（2）搭建：
首先，在cartographer_ros文件夹下的map_builder_bridage.h文件中声明接口函数。
在声明接口函数之前，我们需要先声明使用MapById、NodeId和TrajectoryNodePose，也就是需要返回得到的数据三种类型。在cartographer_ros命名空间下声明就行，详见下面的片段。
namespace cartographer_ros {
using ::cartographer::mapping::NodeId;
using ::cartographer::mapping::MapById;
using ::cartographer::mapping::TrajectoryNodePose;
}
在MapBuilderBridge类下声明公有函数GetTrajectoryNodePoses()，返回类型为std::shared_ptr<MapById<NodeId, TrajectoryNodePose>>。
std::shared_ptr<MapById<NodeId, TrajectoryNodePose>> GetTrajectoryNodePoses();
然后，在cartographer_ros文件夹下的map_builder_bridage.cc文件中实现接口函数。
std::shared_ptr<MapById<NodeId, TrajectoryNodePose>> MapBuilderBridge::GetTrajectoryNodePoses(){
std::shared_ptr<MapById<NodeId,TrajectoryNodePose>> trajectory_node_poses =
std::make_shared<MapById<NodeId,TrajectoryNodePose>>(map_builder_->pose_graph()->GetTrajectoryNodePoses());
return trajectory_node_poses;
}
通过调用MapBuilder类内的pose_graph链接到PoseGraph3D的GetTrajectoryNodePoses()函数，然后返回该智能指针。（这里的链接原理呢，简单来说就是通过读取配置参数，然后将MapBuilder类导向2D或者3D方向）
最后，我们只需要在node类内定义发布者和发布函数，同时自定义相应消息即可。
第一步，在node.h文件中声明函数和对象
void PublishTrajectoryNodePose(const ::ros::WallTimerEvent& timer_event); // 发布函数
::ros::Publisher trajectory_node_pose_publisher_; // 发布者
size_t last_trajectory_nodes_size_ = 0; // 查询id用
absl::Mutex trajectory_node_pose_mutex_; // 做保护用
第二步，在node.cc文件中实现函数
在catographer_ros命名空间内声明所要用的数据类型。
namespace cartographer_ros {
using ::cartographer::mapping::NodeId;
using ::cartographer::mapping::MapById;
using ::cartographer::mapping::TrajectoryNodePose;
using ::cartographer::transform::Rigid3d;
}
在node构造函数内初始化发布者。
if(node_options_.map_builder_options.use_trajectory_builder_3d()){
trajectory_node_pose_publisher_ =
node_handle_.advertise<::my_msgs::trajectorynodepose>(
ktrajectorynodepose,kLatestOnlyPublisherQueueSize);
}
这里注意下，我是自定义了my_msgs::trajectorynodepose消息，至于为什么要再创个功能包来保存自定义的消息，这里我是为了出于工程化的考虑，方便后续更改时能够分辨哪些是我自己创建的，哪些是原本就有的。如果不想创建新的功能包保存自定义消息的朋友可以直接在cartographer_ros_msgs下直接创建就可以，我会在第三步教大家如何创建自己的消息类型。同时这里ktrajectorynodepose是消息名称，统一放在node_constants.h文件下，同样也是出于工程化的考虑。
constexpr char ktrajectorynodepose[] = "trajectory_node_pose";
constexpr double kConstraintPublishPeriodSec = 0.5;
在node构造函数的最后, 为发布位姿函数添加定时器, 设置为kConstraintPublishPeriodSec（此变量可自定义为相应的常量数字，定义在node_constants.h文件下）秒种执行一次。
if(node_options_.map_builder_options.use_trajectory_builder_3d()){
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kConstraintPublishPeriodSec),
&Node::PublishTrajectoryNodePose, this));
}
在node.cc文件内实现 PublishTrajectoryNodePose()函数。
void Node::PublishTrajectoryNodePose(const ::ros::WallTimerEvent& timer_event){
if(trajectory_node_pose_publisher_.getNumSubscribers()>=0){
::my_msgs::trajectorynodepose trajectorynodepose;
constexpr int trajectory_id = 0; // 定义轨迹id
std::shared_ptr<MapById<NodeId, TrajectoryNodePose>> pose = map_builder_bridge_.GetTrajectoryNodePoses();// 调用map_builder_bridge的GetTrajectoryNodePoses函数获取位姿
size_t trajectory_nodes_size = pose->SizeOfTrajectoryOrZero(trajectory_id);// 获取轨迹节点数量
if (last_trajectory_nodes_size_ == trajectory_nodes_size)
return; // 异常处理
last_trajectory_nodes_size_ = trajectory_nodes_size;
absl::MutexLock lock(&trajectory_node_pose_mutex_); // 互斥锁，保护用
auto node_it = pose->BeginOfTrajectory(trajectory_id); // 获取指定id轨迹开始节点
auto end_it = pose->EndOfTrajectory(trajectory_id); // 获取指定id轨迹结束节点
for (; node_it != end_it; ++node_it) {
auto& trajectory_node_pose = pose->at(node_it->id); // 获取轨迹节点位姿
auto& global_pose = trajectory_node_pose.global_pose; // 获取轨迹节点位姿下的全局位姿
const auto time = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count(); // 获取当前时间与时间元年的间隔（秒数）
auto translation = global_pose.translation(); // 获取平移信息
auto rotation = global_pose.rotation(); // 获取旋转信息
// 消息传递
trajectorynodepose.time = time;
trajectorynodepose.x = translation(0);
trajectorynodepose.y = translation(1);
trajectorynodepose.z = translation(2);
trajectorynodepose.q0 = rotation.x();
trajectorynodepose.q1 = rotation.y();
trajectorynodepose.q2 = rotation.z();
trajectorynodepose.q3 = rotation.w();
}
// 消息发布
trajectory_node_pose_publisher_.publish(trajectorynodepose);
}
}
第三步，创建自定义消息（以上述trajectorynodepose为例进行分析）
（1）创建trajectorynodepose.msg
现在src/cartograoher_ros路径下的终端输入如下指令创建功能包：
catkin_create_pkg my_msgs std_msgs rospy roscpp
my_msgs为我的功能包名称，后面是依赖。创建完成后可以看到my_msgs文件夹下有CMakeLists.txt和package.xml。需要对CMakeLists.txt文件进行适当更改，文件的内容如下所示：
CMakeLists.txt：
cmake_minimum_required(VERSION 3.0.2)
project(my_msgs)
find_package(catkin REQUIRED COMPONENTS
message_generation
roscpp
rospy
std_msgs
)
add_message_files(
DIRECTORY msg
FILES
trajectorynodepose.msg
)
generate_messages(
DEPENDENCIES
std_msgs
${PACKAGE_DEPENDENCIES}
)
catkin_package(
CATKIN_DEPENDS message_runtime
）
include_directories(
${catkin_INCLUDE_DIRS}
)
主要是更改add_message_files，使其发现msgs文件夹下的trajectorynodepose.msg。
然后创建msgs文件夹并在该文件夹下创建trajectorynodepose.msg文件。
trajectorynodepose.msg：
# cartographer 后端全局位姿输出
int32 time
float64 x
float64 y
float64 z
float64 q0
float64 q1
float64 q2
float64 q3
如果不需要自定义功能包并在该功能包下自定义消息，可以在cartographer_ros_msgs文件夹下直接定义trajectorynodepose.msg，不需要进行这最后一步。
（2）更改catographer_ros文件
最后需要对catographer_ros功能包下的CMakeLists.txt和package.xml进行适当更改，使其依赖的自定义消息能够成功在程序中找到，避免未声明的错误。
在CMakeLists.txt文件内的set(PACKAGE_DEPENDENCIES处添加my_msgs，如下所示：
set(PACKAGE_DEPENDENCIES
cartographer_ros_msgs
my_msgs
eigen_conversions
geometry_msgs
message_runtime
nav_msgs
pcl_conversions
rosbag
roscpp
roslib
sensor_msgs
std_msgs
tf2
tf
tf2_eigen
tf2_ros
urdf
visualization_msgs
)
在 package.xml文件内添加my_msgs的build依赖，如下所示：
<depend version_gte="1.0.0">cartographer</depend>
<depend version_gte="1.0.0">cartographer_ros_msgs</depend>
<depend>my_msgs</depend>
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
