第三讲 【cartographer】 添加功能以从RVIZ为纯本地化模式设置初始姿势

系列文章目录

第一讲【ROS-SLAM】2D激光雷达 cartographer构建地图

第二讲 【cartographer】Ubuntu16.04 kinetic 最新版cartographer安装（2020/11/4更新）

第三讲 【cartographer】 添加功能以从RVIZ为纯本地化模式设置初始姿势

第四讲 【cartographer】纯定位 纯本地化 pure_localization

第五讲【cartographer】在仿真环境中 建图 纯定位

第六讲【cartographer】纯定位参数优化（初级篇）

---

---

前言

cartographer作为一个谷歌开源的项目，在SLAM建图上拥有较好的性能，既然有建图的功能，也会有定位的功能，下面是讲述cartographer纯定位模式下的，添加功能以从RVIZ为纯本地化模式设置初始姿势。

---

一、RVIZ为纯本地化模式设置初始姿势

如果将cartographer作为一个定位算法使用，那么在RVIZ为纯本地化模式设置初始姿势必不可少。相比amcl，cartographer的很多功能并不完善，需要自己寻找代价添加功能。

顺便说一句，对于一个新入门的程序员来说，cartographer的纯本地化模式是比较难以掌握的，需要我们一步步解决各种问题。

二、使用步骤

1.引入库

1）cartographer_ros / CMakeLists.txt

添加

tf2_geometry_msgs

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 
#include 

#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