第三讲 【cartographer】 添加功能以从RVIZ为纯本地化模式设置初始姿势
第一讲【ROS-SLAM】2D激光雷达 cartographer构建地图
第二讲 【cartographer】Ubuntu16.04 kinetic 最新版cartographer安装（2020/11/4更新）
第三讲 【cartographer】 添加功能以从RVIZ为纯本地化模式设置初始姿势
第四讲 【cartographer】纯定位 纯本地化 pure_localization
第五讲【cartographer】在仿真环境中 建图 纯定位
文章目录
前言
cartographer作为一个谷歌开源的项目，在SLAM建图上拥有较好的性能，既然有建图的功能，也会有定位的功能，下面是讲述cartographer纯定位模式下的，添加功能以从RVIZ为纯本地化模式设置初始姿势。一、RVIZ为纯本地化模式设置初始姿势
如果将cartographer作为一个定位算法使用，那么在RVIZ为纯本地化模式设置初始姿势必不可少。相比amcl，cartographer的很多功能并不完善，需要自己寻找代价添加功能。
顺便说一句，对于一个新入门的程序员来说，cartographer的纯本地化模式是比较难以掌握的，需要我们一步步解决各种问题。
二、使用步骤
1.引入库
1）cartographer_ros / CMakeLists.txt
添加
tf2_geometry_msgs
- 1
2）cartographer_ros/cartographer_ros/CMakeLists.txt
添加
google_binary(set_initpose_from_rviz
SRCS
set_initpose_main.cc
)
install(TARGETS set_initpose_from_rviz
ARCHIVE DESTINATION ${
CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${
CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${
CATKIN_PACKAGE_BIN_DESTINATION}
)
3）cartographer_ros/launch/demo_backpack_2d_localization.launch
启动文件
<node name="set_initpose" pkg="cartographer_ros" type="set_initpose_from_rviz" output="screen"
args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_localization.lua
-load_state_filename $(arg load_state_filename)" >
</node>
4）cartographer_ros/package.xml
添加
<depend>tf2_geometry_msgs</depend>
2.添加功能代码
代码如下：
cartographer_ros/cartographer_ros/set_initpose_main.cc
/*
* Copyright 2019 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <string>
#include <vector>
#include “cartographer/common/configuration_file_resolver.h”
#include “cartographer/io/proto_stream.h”
#include “cartographer/mapping/map_builder.h”
#include “cartographer_ros/node_constants.h”
#include “cartographer_ros/node_options.h”
#include “cartographer_ros/ros_log_sink.h”
#include “cartographer_ros_msgs/FinishTrajectory.h”
#include “cartographer_ros_msgs/GetTrajectoryStates.h”
#include “cartographer_ros_msgs/StartTrajectory.h”
#include “cartographer_ros_msgs/StatusCode.h”
#include “geometry_msgs/PoseWithCovarianceStamped.h”
#include “gflags/gflags.h”
#include “ros/ros.h”
#include “std_msgs/String.h”
#include “tf2_geometry_msgs/tf2_geometry_msgs.h”
DEFINE_string(configuration_directory, “”,
"First directory in which configuration files are searched, "
"second is always the Cartographer installation to allow "
“including files from there.”);
DEFINE_string(configuration_basename, “”,
"Basename, i.e. not containing any directory prefix, of the "
“configuration file.”);
DEFINE_string(load_state_filename, “”,
“Filename of a pbstream to draw a map from.”);
namespace {
std::unique_ptr<cartographer::mapping::MapBuilder> map_builder_;
} // namespace
// subscribe callback function from Rviz
void move_base_simple_callback(
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) {
::ros::NodeHandle nh;
// find the active active trajectory
::ros::ServiceClient client_get_trajectroy_states =
nh.serviceClient<cartographer_ros_msgs::GetTrajectoryStates>(
cartographer_ros::kGetTrajectoryStatesServiceName);
cartographer_ros_msgs::GetTrajectoryStates srv_get_trajectroy_states;
if (!client_get_trajectroy_states.call(srv_get_trajectroy_states)) {
LOG(ERROR) << "Failed to call "
<< cartographer_ros::kGetTrajectoryStatesServiceName << “.”;
}
if (srv_get_trajectroy_states.response.status.code !=
cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << “Error get trajectory states - message: '”
<< srv_get_trajectroy_states.response.status.message
<< "’ (status code: "
<< std::to_string(srv_get_trajectroy_states.response.status.code)
<< “).”;
return;
}
int current_trajectory_id = -1;
for (size_t i = 0; i < srv_get_trajectroy_states.response.trajectory_states
.trajectory_state.size();
i++) {
if (srv_get_trajectroy_states.response.trajectory_states.trajectory_state
.at(i) == cartographer_ros_msgs::TrajectoryStates::ACTIVE)
current_trajectory_id =
srv_get_trajectroy_states.response.trajectory_states.trajectory_id.at(
i);
}
if (current_trajectory_id == -1) {
LOG(ERROR) << “No active trajectory!”;
return;
}
// stop the current active trajectory
::ros::ServiceClient client_finish_trajectroy =
nh.serviceClient<cartographer_ros_msgs::FinishTrajectory>(
cartographer_ros::kFinishTrajectoryServiceName);
cartographer_ros_msgs::FinishTrajectory srv_finish_trajectroy;
srv_finish_trajectroy.request.trajectory_id = current_trajectory_id++;
if (!client_finish_trajectroy.call(srv_finish_trajectroy)) {
LOG(ERROR) << "Failed to call "
<< cartographer_ros::kFinishTrajectoryServiceName << “.”;
}
if (srv_finish_trajectroy.response.status.code !=
cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << “Error finishing trajectory - message: '”
<< srv_finish_trajectroy.response.status.message
<< "’ (status code: "
<< std::to_string(srv_finish_trajectroy.response.status.code)
<< “).”;
return;
}
// start a new trajectory
const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();
// get the start pose of the trajectory0 w.r.t /map
const auto traj_ref_pose = node_poses.BeginOfTrajectory(0)->data.global_pose;
tf2::Transform traj_ref_tf;
traj_ref_tf.getOrigin() = tf2::Vector3(traj_ref_pose.translation().x(),
traj_ref_pose.translation().y(),
traj_ref_pose.translation().z());
traj_ref_tf.setRotation(tf2::Quaternion(
traj_ref_pose.rotation().x(), traj_ref_pose.rotation().y(),
traj_ref_pose.rotation().z(), traj_ref_pose.rotation().w()));
// get init pose w.r.t /map
tf2::Transform map_tf;
tf2::fromMsg(msg->pose.pose, map_tf);
tf2::Transform relative_initpose_tf = traj_ref_tf.inverse() * map_tf;
// set the start trajectory service call
::ros::ServiceClient client_start_trajectroy =
nh.serviceClient<cartographer_ros_msgs::StartTrajectory>(
cartographer_ros::kStartTrajectoryServiceName);
cartographer_ros_msgs::StartTrajectory srv_start_trajectroy;
srv_start_trajectroy.request.configuration_directory =
FLAGS_configuration_directory;
srv_start_trajectroy.request.configuration_basename =
FLAGS_configuration_basename;
srv_start_trajectroy.request.relative_to_trajectory_id =
0; // frozen trajectory
srv_start_trajectroy.request.use_initial_pose = true;
tf2::toMsg(relative_initpose_tf, srv_start_trajectroy.request.initial_pose);
if (!client_start_trajectroy.call(srv_start_trajectroy)) {
LOG(ERROR) << "Failed to call "
<< cartographer_ros::kStartTrajectoryServiceName << “.”;
}
if (srv_start_trajectroy.response.status.code !=
cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << “Error starting trajectory - message: '”
<< srv_start_trajectroy.response.status.message
<< "’ (status code: "
<< std::to_string(srv_start_trajectroy.response.status.code)
<< “).”;
}
}
int main(int argc, char argv) {
google::InitGoogleLogging(argv[0]);
google::SetUsageMessage(
“\n\n”
"Convenience tool around the start_trajectory service. This takes a Lua "
"file that is accepted by the node as well and starts a new trajectory "
“using its settings.\n”);
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_configuration_basename.empty())
<< “-configuration_basename is missing.”;
CHECK(!FLAGS_configuration_directory.empty())
<< “-configuration_directory is missing.”;
// load pbstream
::cartographer::io::ProtoStreamReader reader(FLAGS_load_state_filename);
cartographer_ros::NodeOptions node_options;
std::tie(node_options, std::ignore) = cartographer_ros::LoadOptions(
FLAGS_configuration_directory, FLAGS_configuration_basename);
map_builder_ = absl::make_unique<cartographer::mapping::MapBuilder>(
node_options.map_builder_options);
map_builder_->LoadState(&reader, true);
::ros::init(argc, argv, “cartographer_start_trajectory”);
::ros::start();
::ros::NodeHandle nh;
::ros::Subscriber sub_move_base_simple =
nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>(
“/initialpose”, 1, &move_base_simple_callback);
::ros::spin();
}
总结
以上就是今天要讲的内容，本文介绍了cartographer纯本地化模式下如何设置初始姿势的功能。
参考链接：https://github.com/cartographer-project/cartographer_ros/pull/1284/files