学习公司cartographer改动代码之入口差异
文章目录
前言
由于这段时间学习公司自研cartographer的内容，有很多内容代码与原使用方式不同，例如：本章的配置文件加载方式与原cartographer加载方式的不同，接下来我先以原cartographer的使用方式，然后再以我们自研的作对比。
一、代码入口
1.原cartographer
cartographer原代码入口主要以node_main.cc和node_grpc_main.cc，由于node_grpc_main.cc不怎么使用，这里就不再赘述了；查看文件主要先看main函数，
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);// 初始化 glog
google::ParseCommandLineFlags(&argc, &argv, true);// 初始化 gflags
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
::ros::init(argc, argv, "cartographer_node");
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;//使谷歌日志记录(glog)使用ROS日志记录输出
cartographer_ros::Run();
::ros::shutdown();
}
而前几行都是ros和google log使用，其中真正步入正题内容的函数就是cartographer_ros::Run()调用，进入Run()函数,具体代码作用以注释
void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};//tf变换的历史要保存10秒
tf2_ros::TransformListener tf(tf_buffer);//负责tf接收
NodeOptions node_options;//节点参数
TrajectoryOptions trajectory_options;//轨迹参数
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);//std::tie把node_options和trajectory_options整合成一个std::tuple,从而实现批量赋值（node_options和 trajectory_options一起赋值）
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主函数，这里是一个关键步骤，如果没有这一行代码，很多话题的发布，服务的调用都是不能实现的。
if (!FLAGS_load_state_filename.empty()) {// 如果加载了pbstream文件, 就执行这个函数
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
if (FLAGS_start_trajectory_with_default_topics) {//默认使用topic开始轨迹，一开始是true
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
::ros::spin();
node.FinishAllTrajectories();// 结束所有处于活动状态的轨迹
node.RunFinalOptimization();// 当所有的轨迹结束时, 再执行一次全局优化
if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}
}
原cartographer的入口差不多就这些了，下面进行我们修改的讲解。
2.修改cartographer
最开始我以为我们的入口是node_grpc_main.cc，但后来发现也不对，我们的node_main.cc文件做了大量修改，应该就是这个部分，但是入口函数文件代码却完全不一样，以下是main函数
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
//LOG(INFO) << "Peak.ding FLAGS_configuration_directory " << FLAGS_configuration_directory;
//LOG(INFO) << "Peak.ding FLAGS_configuration_slam_basename " << FLAGS_configuration_slam_basename;
//LOG(INFO) << "Peak.ding FLAGS_configuration_localization_basename " << FLAGS_configuration_localization_basename;
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_slam_basename.empty())
<< "-FLAGS_configuration_slam_basename is missing.";
CHECK(!FLAGS_configuration_localization_basename.empty())
<< "-FLAGS_configuration_localization_basename is missing.";
::ros::init(argc, argv, "cartographer_node");
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::Run();
::ros::shutdown();
}
其中main()函数与原cartographer的一致，我这里就不再解释，把目光转到Run()函数里面
void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
tf2_ros::TransformListener tf(tf_buffer);
NodeOptions node_options;
//node_options = LoadNodeOptions(FLAGS_configuration_directory,
// FLAGS_configuration_basename_server);
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory,
FLAGS_configuration_basename_server);
//auto map_builder = absl::make_unique<cartographer::mapping::MapBuilder>(
// node_options.map_builder_options);
cartographer::cloud::proto::MapBuilderServerOptions
map_builder_server_options =
cartographer::cloud::LoadMapBuilderServerOptions(
FLAGS_configuration_directory,
FLAGS_configuration_basename_server);
//LOG(INFO) << "Peak.ding map_builder";
auto map_builder =
std::make_shared<cartographer::mapping::MapBuilder>( // okagv default is
// absl::make_unique
const_cast<::cartographer::mapping::proto::MapBuilderOptions&>(map_builder_server_options.map_builder_options()));
std::unique_ptr<cartographer::cloud::MapBuilderServerInterface>
map_builder_server =
absl::make_unique<cartographer::cloud::MapBuilderServer>(
map_builder_server_options, map_builder);
//LOG(INFO) << "Peak.ding map_builder_server start";
map_builder_server->Start();
//Node node(node_options, std::move(map_builder), &tf_buffer, FLAGS_collect_metrics);
//LOG(INFO) << "Peak.ding Node start";
Node node(node_options, map_builder, &tf_buffer, FLAGS_collect_metrics);
/*
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
*/
//LOG(INFO) << "Peak.ding root_file_directory " << node_options.root_file_directory;
if (!FLAGS_configuration_directory.empty()) {
node.SetConfigurationParam(FLAGS_configuration_directory,
FLAGS_configuration_basename_server);
}
if (!node_options.root_file_directory.empty()) {
//node.LoadMaps(node_options.root_file_directory + "/map");
}
if (FLAGS_start_trajectory_with_default_topics) {
// node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
//ros::AsyncSpinner async_spinner(2);
//async_spinner.start();
//::ros::waitForShutdown();
//ros::MultiThreadedSpinner spinner(2);
//spinner.spin();
//LOG(INFO) << "Peak.ding Ros Start";
::ros::spin();
map_builder_server->Shutdown();
node.FinishAllTrajectories();
node.RunFinalOptimization();
if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}
}
因这部分与原cartographer不同，这里我就一点一点的解释
cartographer::cloud::proto::MapBuilderServerOptions
map_builder_server_options =
cartographer::cloud::LoadMapBuilderServerOptions(
FLAGS_configuration_directory,
FLAGS_configuration_basename_server);
上面这部分代码是为了获得map_builder_server_options,作用是为了后面的map_builder和map_builder_server，代码如下：
auto map_builder =
std::make_shared<cartographer::mapping::MapBuilder>( // okagv default is
// absl::make_unique
const_cast<::cartographer::mapping::proto::MapBuilderOptions&>(map_builder_server_options.map_builder_options()));
std::unique_ptr<cartographer::cloud::MapBuilderServerInterface>
map_builder_server =
absl::make_unique<cartographer::cloud::MapBuilderServer>(
map_builder_server_options, map_builder);
这里的map_builder_server在原cartographer中是没有的，这里主要是使用来启动服务线程，代码如下：
map_builder_server->Start();
下面这一句代码与原cartographer相同，不再讲述
Node node(node_options, map_builder, &tf_buffer, FLAGS_collect_metrics);
下面这一句代码是我们所改动部分Run()函数的核心，作用是将FLAGS_configuration_directory和FLAGS_configuration_basename_server两个变量传给node中
if (!FLAGS_configuration_directory.empty()) {
node.SetConfigurationParam(FLAGS_configuration_directory,
FLAGS_configuration_basename_server);
}
接下来的部分代码差别就在于map_builder_server->Shutdown()，这句话的意思就是关闭map_builder_server的线程服务；其它部分与原cartographer一致，到这里我们的入口程序讲完了，但很关键的一些东西完全还没有说呢。
二、本章重点之配置文件读取
与之前一样分两部分阐述
1.原cartographer创建轨迹
上一节我有一句代码没有解释，但它却是原cartographer入口的核心
node.StartTrajectoryWithDefaultTopics(trajectory_options);
通过转到定义可以看到在node.cc文件中的这个函数
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);//上🔓
CHECK(ValidateTrajectoryOptions(options));//检查
AddTrajectory(options);//核心语句
}
其中核心语句是AddTrajectory(options);原代码也在node.cc中
int Node::AddTrajectory(const TrajectoryOptions& options) {
const
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);//传感器id,有些同一类多个传感器 const int
trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids,
options);//通过map_builder_bridge_添加轨迹
AddExtrapolator(trajectory_id, options);//添加外推器
AddSensorSamplers(trajectory_id, options);//采样器
LaunchSubscribers(options, trajectory_id);// 订阅话题与注册回调函数
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec),
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true)); for (const auto& sensor_id :
expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id); } return trajectory_id; }
其中核心语句是，由于篇幅太大这里只详细解释第一种
1、ComputeExpectedSensorIds(options)确定传感器topic，如果有多个相同类型的传感器，则使用话题_数字的方式
2、AddTrajectory(expected_sensor_ids, options)增加轨迹
3、AddExtrapolator(trajectory_id, options)配置外推周期
4、AddSensorSamplers(trajectory_id, options)配置采样器采样率
5、LaunchSubscribers(options, trajectory_id)订阅话题与注册回调
ComputeExpectedSensorIds函数
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
using SensorType = SensorId::SensorType;
std::set<SensorId> expected_topics;
// Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
for (const std::string& topic :
ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
for (const std::string& topic : ComputeRepeatedTopicNames(
kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
for (const std::string& topic :
ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
expected_topics.insert(SensorId{SensorType::IMU, kImuTopic});
}
// Odometry is optional.
if (options.use_odometry) {
expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic});
}
// NavSatFix is optional.
if (options.use_nav_sat) {
expected_topics.insert(
SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic});
}
// Landmark is optional.
if (options.use_landmarks) {
expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
}
return expected_topics;
}
这段代码实现的功能就是，创建一个set类expected_topics，向其中推送各种传感器的SensorId，其详情这里就不阐述了。
2.修改的cartographer创建轨迹
修改后的cartographer创建轨迹却与原cartographer不一样，上面部分的阐述中主要是从node_main.cc的node.StartTrajectoryWithDefaultTopics(trajectory_options);作为调用函数，而我们自有的却是被注释掉了的，这里我花了很长时间才搞明白。这里再把那部分代码发出来看看
void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
tf2_ros::TransformListener tf(tf_buffer);
NodeOptions node_options;
//node_options = LoadNodeOptions(FLAGS_configuration_directory,
// FLAGS_configuration_basename_server);
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory,
FLAGS_configuration_basename_server);
//auto map_builder = absl::make_unique<cartographer::mapping::MapBuilder>(
// node_options.map_builder_options);
cartographer::cloud::proto::MapBuilderServerOptions
map_builder_server_options =
cartographer::cloud::LoadMapBuilderServerOptions(
FLAGS_configuration_directory,
FLAGS_configuration_basename_server);
//LOG(INFO) << "Peak.ding map_builder";
auto map_builder =
std::make_shared<cartographer::mapping::MapBuilder>( // okagv default is
// absl::make_unique
const_cast<::cartographer::mapping::proto::MapBuilderOptions&>(map_builder_server_options.map_builder_options()));
std::unique_ptr<cartographer::cloud::MapBuilderServerInterface>
map_builder_server =
absl::make_unique<cartographer::cloud::MapBuilderServer>(
map_builder_server_options, map_builder);
//LOG(INFO) << "Peak.ding map_builder_server start";
map_builder_server->Start();
//Node node(node_options, std::move(map_builder), &tf_buffer, FLAGS_collect_metrics);
//LOG(INFO) << "Peak.ding Node start";
Node node(node_options, map_builder, &tf_buffer, FLAGS_collect_metrics);
/*
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
*/
//LOG(INFO) << "Peak.ding root_file_directory " << node_options.root_file_directory;
if (!FLAGS_configuration_directory.empty()) {
node.SetConfigurationParam(FLAGS_configuration_directory,
FLAGS_configuration_basename_server);
}
if (!node_options.root_file_directory.empty()) {
//node.LoadMaps(node_options.root_file_directory + "/map");
}
if (FLAGS_start_trajectory_with_default_topics) {
// node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
//ros::AsyncSpinner async_spinner(2);
//async_spinner.start();
//::ros::waitForShutdown();
//ros::MultiThreadedSpinner spinner(2);
//spinner.spin();
//LOG(INFO) << "Peak.ding Ros Start";
::ros::spin();
map_builder_server->Shutdown();
node.FinishAllTrajectories();
node.RunFinalOptimization();
if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}
}
注释掉了，那肯定就是其它地方作为入口了，这里我们把入口放在了服务调用里面去了，而服务调用就在Node函数中，调用代码如下
Node node(node_options, map_builder, &tf_buffer, FLAGS_collect_metrics);
这里之前有介绍过，这里把node函数展示出来
Node::Node(
const NodeOptions& node_options,
std::shared_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_,
map_builder, //std::move(map_builder),
tf_buffer) {
absl::MutexLock lock(&mutex_);
if (collect_metrics) {
metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
carto::metrics::RegisterAllMetrics(metrics_registry_.get());
}
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, kLatestOnlyPublisherQueueSize);
trajectory_node_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
landmark_poses_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
constraint_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kConstraintListTopic, kLatestOnlyPublisherQueueSize);
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));
// okagv
service_servers_.push_back(node_handle_.advertiseService(
kLoadTrajectoryServiceName, &Node::HandleLoadTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kDeleteTrajectoryServiceName, &Node::HandleDeleteTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kLocalizeTrajectoryServiceName, &Node::HandleLocalizeTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kSetTrajectoryStatesServiceName, &Node::HandleSetTrajectoryStates, this));
service_servers_.push_back(node_handle_.advertiseService(
kUpdateStateServiceName, &Node::HandleUpdateState, this));
scan_matched_point_cloud_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>(
kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.submap_publish_period_sec),
&Node::PublishSubmapList, this));
if (node_options_.pose_publish_period_sec > 0) {
publish_local_trajectory_data_timer_ = node_handle_.createTimer(
::ros::Duration(node_options_.pose_publish_period_sec),
&Node::PublishLocalTrajectoryData, this);
}
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishTrajectoryNodeList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishLandmarkPosesList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kConstraintPublishPeriodSec),
&Node::PublishConstraintList, this));
// okagv
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kConstraintPublishPeriodSec),
&Node::CheckAndRunOKagvOrder, this));
// okagv
initial_pose_subscriber_ = node_handle_.subscribe(
"/initialpose", 1, &Node::HandleInitialPoseMessage, this);
// okagv
current_pose_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::RobotPose>(
kRobotPoseTopic, kLatestOnlyPublisherQueueSize);
timer_thread_ = new std::thread(boost::bind(&Node::StartRelocalizeRobot, this));
//okagv
service_servers_.push_back(node_handle_.advertiseService(
kLaserScanQualityServiceName, &Node::HandleScanQualityQuery, this));
// okagv
rosbag_start_recorder_ =
node_handle_.advertise<::cartographer_ros_msgs::Rosbag>(
kRosbagStartRecordTopic, kLatestOnlyPublisherQueueSize);
// okagv
rosbag_stop_recorder_ =
node_handle_.advertise<std_msgs::String>(
kRosbagStopRecordTopic, kLatestOnlyPublisherQueueSize);
}
其中服务调用在如下代码之中
service_servers_.push_back(node_handle_.advertiseService(
kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
这里面的HandleStartTrajectory是一个ros的服务处理函数，看函数在node.cc文件中
bool Node::HandleStartTrajectory(
::cartographer_ros_msgs::StartTrajectory::Request& request,
::cartographer_ros_msgs::StartTrajectory::Response& response) {
//absl::MutexLock lock(&mutex_);
if (is_service_done == true) {
is_service_done = false;
//
absl::SleepFor(absl::Seconds(0.1));
} else {
response.status.code = cartographer_ros_msgs::StatusCode::UNIMPLEMENTED;
response.status.message = "another service is running, please wait";
// is_service_done = f;
// current_trajectory_type = TrajectoryType::IDLE;
return true;
}
switch (current_trajectory_type) {
case TrajectoryType::SLAM:
LOG(INFO) << "HandleStartTrajectory SLAM";
// finish and delete current trajectory_id;
if (!FinishAndDeleteTrajectory(last_trajectory_id_name)) {
response.status.code = cartographer_ros_msgs::StatusCode::ABORTED;
response.status.message =
"last trajectory is in slam mode, and remove it fail";
return true;
}
/*
if (request.use_initial_pose == true) {
// finish and delete current trajectory_id;
if (!FinishAndDeleteTrajectory(last_trajectory_id_name)) {
response.status.code = cartographer_ros_msgs::StatusCode::ABORTED;
response.status.message =
"last trajectory is in slam mode, and remove it fail";
return true;
}
} else {
if (!trajectory_id_toint.count(last_trajectory_id_name)) {
response.status.code = cartographer_ros_msgs::StatusCode::ABORTED;
response.status.message = "no trajectory found, and finish it fail";
return true;
}
if (!FinishTrajectory(trajectory_id_toint[last_trajectory_id_name])) {
response.status.code = cartographer_ros_msgs::StatusCode::ABORTED;
response.status.message =
"last trajectory is in slam mode, and finish it fail";
return true;
}
}
*/
break;
case TrajectoryType::NAVIGATION:
LOG(INFO) << "HandleStartTrajectory NAVIGATION";
// finish and delete current navigation trajectory_id;
if (!FinishAndDeleteTrajectory(last_trajectory_id_name)) {
response.status.code = cartographer_ros_msgs::StatusCode::ABORTED;
response.status.message =
"last trajectory is in navigation mode, and remove it fail";
return true;
}
// finsih and delete current map trajectory_id
if (!FinishAndDeleteTrajectory(last_map_trajectory_id_name)) {
response.status.code = cartographer_ros_msgs::StatusCode::ABORTED;
response.status.message =
"last trajectory is in navigation mode, and remove it fail";
return true;
}
break;
case TrajectoryType::IDLE:
LOG(INFO) << "HandleStartTrajectory IDLE";
// finsih and delete current map trajectory_id
LOG(INFO) << "last_map_trajectory_id_name " << last_map_trajectory_id_name;
if (request.use_initial_pose == true) break;
if (!last_trajectory_id_name.empty()) {
if (!FinishAndDeleteTrajectory(last_map_trajectory_id_name)) {
response.status.code = cartographer_ros_msgs::StatusCode::ABORTED;
response.status.message = "has last trajectory but remove it fail";
return true;
}
}
break;
default:
break;
}
if(request.use_initial_pose == true)
{
request.relative_to_trajectory_id = last_trajectory_id_name;
request.initial_pose = ToGeometryMsgPose(current_tracking_to_map);
}
// start new slam
HandleStartTrajectoryDetail(request, response);
if (response.status.code == cartographer_ros_msgs::StatusCode::OK) {
current_trajectory_type = TrajectoryType::SLAM;
LOG(INFO) << "Success to start Trajectory";
cartographer_ros_msgs::Rosbag order;
order.config = "standard";
order.bag_name = request.trajectory_id;
rosbag_start_recorder_.publish(order);
OKagv_Feedback feedback;
feedback.code = 0;
feedback.state = OKagv_State::SUCCESS;
map_builder_bridge_.SetOKagv_Feedback(feedback);
} else {
current_trajectory_type = TrajectoryType::ABORTION;
LOG(INFO) << "Fail to start Trajectory";
OKagv_Feedback feedback;
feedback.code = 1;
feedback.state = OKagv_State::SUCCESS;
feedback.message = "Fail to start Trajectory";
map_builder_bridge_.SetOKagv_Feedback(feedback);
}
absl::SleepFor(absl::Seconds(node_options_.time_delay_for_start_trajectory));
is_service_done = true;
return true;
}
这其中主要的函数在
// start new slam
HandleStartTrajectoryDetail(request, response);
这个函数也在node.cc文件中
bool Node::HandleStartTrajectoryDetail(
::cartographer_ros_msgs::StartTrajectory::Request& request,
::cartographer_ros_msgs::StartTrajectory::Response& response) {
TrajectoryOptions trajectory_options;
std::tie(std::ignore, trajectory_options) =
LoadOptions(configuration_directory_, configuration_basename_);
if (configuration_directory_.empty() || configuration_basename_.empty()) {
response.status.message = "configuration file do not find";
return true;
} else {
//LOG(INFO) << "Peak.ding check configuration_directory_ "
// << configuration_directory_;
//LOG(INFO) << "Peak.ding check configuration_basename_ "
// << configuration_basename_;
}
//
int scheduled_trajectory_id = 0;
std::string scheduled_trajectory_id_name;
auto it = trajectory_id_toint.find(request.trajectory_id);
if (it == trajectory_id_toint.end()) {
// okagv
if (request.trajectory_type == "slam") {
scheduled_trajectory_id = trajectory_id_toint.size();
scheduled_trajectory_id_name = request.trajectory_id;
if (scheduled_trajectory_id_name.empty())
scheduled_trajectory_id_name = std::to_string(scheduled_trajectory_id);
scheduled_trajectory_type = TrajectoryType::SLAM;
// set slam param
trajectory_options.trajectory_builder_options.set_pure_localization(
false);
} else if (request.trajectory_type == "navigation") {
scheduled_trajectory_id = 1001;
scheduled_trajectory_id_name = "navigation";
scheduled_trajectory_type = TrajectoryType::NAVIGATION;
// set navigation param
trajectory_options.trajectory_builder_options.set_pure_localization(true);
} else {
response.status.message = "Failed. trajectory_type don't exist";
return true;
}
} else {
scheduled_trajectory_id = it->second;
}
if (request.use_initial_pose) {
const auto pose = ToRigid3d(request.initial_pose);
if (!pose.IsValid()) {
response.status.message =
"Invalid pose argument. Orientation quaternion must be normalized.";
LOG(ERROR) << response.status.message;
response.status.code =
cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
LOG(ERROR) << "intial pose is wrong";
return true;
}
// Check if the requested trajectory for the relative initial pose exists.
if (!trajectory_id_toint.count(request.relative_to_trajectory_id)) {
response.status.code =
cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
LOG(ERROR) << "relative_to_trajectory_id is "
<< request.relative_to_trajectory_id
<< ", do not exist map trajectory_id";
return true;
}
int relative_to_trajectory_id =
trajectory_id_toint.at(request.relative_to_trajectory_id);
//LOG(INFO) << "Peak.ding relative_to_trajectory_id " << relative_to_trajectory_id;
response.status = TrajectoryStateToStatus(
relative_to_trajectory_id,
{TrajectoryState::ACTIVE, TrajectoryState::FROZEN,
TrajectoryState::FINISHED, TrajectoryState::IDLE} /* valid states */);
if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Can't start a trajectory with initial pose: "
<< response.status.message;
return true;
}
::cartographer::mapping::proto::InitialTrajectoryPose
initial_trajectory_pose;
initial_trajectory_pose.set_to_trajectory_id(relative_to_trajectory_id);
*initial_trajectory_pose.mutable_relative_pose() =
cartographer::transform::ToProto(pose);
initial_trajectory_pose.set_timestamp(cartographer::common::ToUniversal(
::cartographer_ros::FromRos(ros::Time(0))));
*trajectory_options.trajectory_builder_options
.mutable_initial_trajectory_pose() = initial_trajectory_pose;
}
// First, check if we can actually start the trajectory.
cartographer_ros_msgs::StatusResponse status_response =
TrajectoryStateToStatus(scheduled_trajectory_id, {} /* valid states */);
if (status_response.code != cartographer_ros_msgs::StatusCode::NOT_FOUND) {
LOG(INFO) << "Peak.ding scheduled_trajectory_id " << scheduled_trajectory_id;
LOG(ERROR) << "Can't start trajectory that is exist ";
response.status.code = cartographer_ros_msgs::StatusCode::ALREADY_EXISTS;
response.status.message = "trajectory id already exist";
return true;
}
if (!ValidateTrajectoryOptions(trajectory_options)) {
response.status.message = "Invalid trajectory options.";
LOG(ERROR) << response.status.message;
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
} else if (!ValidateTopicNames(trajectory_options)) {
response.status.message = "Topics are already used by another trajectory.";
LOG(ERROR) << response.status.message;
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
} else {
response.status.message = "Success";
// okagv
SetTrajectoryTypeWithId(request.trajectory_type, scheduled_trajectory_id);
response.trajectory_id =
AddTrajectoryWithId(scheduled_trajectory_id, trajectory_options);
response.status.code = cartographer_ros_msgs::StatusCode::OK;
//okagv
RegisterClientIdForTrajectory(scheduled_trajectory_id,scheduled_trajectory_id_name);
// okagv
last_trajectory_id = scheduled_trajectory_id;
last_trajectory_id_name = scheduled_trajectory_id_name;
trajectory_id_toint.emplace(last_trajectory_id_name,
last_trajectory_id);
current_trajectory_type = scheduled_trajectory_type;
}
return true;
}
这里有很多东西我还没有细看，不过我们只需要找到与原cartographer相似的方法
其代码如下：
response.trajectory_id =
AddTrajectoryWithId(scheduled_trajectory_id, trajectory_options);
通过转到定义可以发现代码
int Node::AddTrajectoryWithId(const int trajectory_id,
const TrajectoryOptions& options) {
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);
map_builder_bridge_.AddTrajectoryWithId(trajectory_id, expected_sensor_ids,
options);
AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, trajectory_id);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec),
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}
是不是有几分熟悉，我们在上一小节就有其中的介绍
1、ComputeExpectedSensorIds(options)确定传感器topic，如果有多个相同类型的传感器，则使用话题_数字的方式
2、AddTrajectory(expected_sensor_ids, options)增加轨迹
3、AddExtrapolator(trajectory_id, options)配置外推周期
4、AddSensorSamplers(trajectory_id, options)配置采样器采样率
5、LaunchSubscribers(options, trajectory_id)订阅话题与注册回调
总结
到这里，初步相同的地方和不同的地方都介绍了很多，下一片继续围绕几个处理函数介绍。
1068
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
