Cartographer保存建图轨迹
需求:从cartographer建好的地图中保存建图轨迹。
思路:在使用cartographer建图的过程中,找到cartographer 提供的 “/trajectory_query” 服务(可以用“rosrun rqt_service_caller rqt_service_caller”命令),请求后会得到当前轨迹每一帧的位姿。在源码中找到这部分内容,将其保存为txt文件即可。
方法:源码node.cc中,修改HandleTrajectoryQuery函数:
bool Node::HandleTrajectoryQuery(
::cartographer_ros_msgs::TrajectoryQuery::Request& request,
::cartographer_ros_msgs::TrajectoryQuery::Response& response) {
absl::MutexLock lock(&mutex_);
response.status = TrajectoryStateToStatus(
request.trajectory_id,
{TrajectoryState::ACTIVE, TrajectoryState::FINISHED,
TrajectoryState::FROZEN} /* valid states */);
if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Can't query trajectory from pose graph: "
<< response.status.message;
return true;
}
map_builder_bridge_.HandleTrajectoryQuery(request, response);
// 保存轨迹为txt文件
std::cout << "receiving" << std::endl;
std::string txt_filename = "/home/xxx/xxx.txt";
std::ofstream outf(txt_filename, std::ios::app);
for (int i = 0; i < response.trajectory.size(); i++){
int32_t sec = response.trajectory[i].header.stamp.sec;
int32_t nsec = response.trajectory[i].header.stamp.nsec;
double x = response.trajectory[i].pose.position.x;
double y = response.trajectory[i].pose.position.y;
double z = response.trajectory[i].pose.position.z;
outf << std::setprecision(20) << sec << "." << nsec << " " << x << " " << y << " " << z << std::endl;
}
return true;
}
效果:
轨迹:
点云+轨迹: