准备工作：安装ROS和cartographer，推荐去鱼香ros一建安装，非常的方便。

你是否遇到如下情况：下班后机器人在一个固定的位置停靠，该位置非建图的原点。开机后机器人会默认出现在建图的起始原点。这两个点距离较远，超过了POSE_GRAPH.constraint_builder.max_constraint_distance=12 设定值（12m）。不能快速定位，超过POSE_GRAPH.global_constraint_search_after_n_seconds = 100.的设定值（超过100s）后，cartographer开启全局搜索，此时开始占用大量计算机资源，容易出现卡死甚至定位失败。本文通过修改cartographer实现将机器人的起始位置设定位原点以外的固定位置，在此周围进行搜索位置可实现快速的定位（以下方法经过实体验证）。

①启动机器人系统，让机器人处于定位成功的状态（当前机器人与历史轨迹建立约束）

②将机器人开到初始位置，新建一个终端输入rostopic echo/odom 或者 rosrun tf_echo /odom /base_link 查看当前位姿，查询结果如下图。查询完成记录下来，所有终端程序就可以关闭了。

Translation分别为 x y z坐标

Rotation分别为姿态的四元数

③在cartographe安装目录中逐级找到node_main.cc 这个文件。

④直接将以下内容复制并替换node_main.cc 中的所有内容（弟兄们直接复制替换即可）

​
/*
 * Copyright 2016 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 "absl/memory/memory.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "tf2_ros/transform_listener.h"
#include "cartographer_ros/msg_conversion.h"

template<typename T>
T getParam_Func(ros::NodeHandle& pnh,
                    const std::string& param_name, const T & default_val)
{
  T param_val;
  pnh.param<T>(param_name, param_val, default_val);
  return param_val;
}


DEFINE_bool(collect_metrics, false,
            "Activates the collection of runtime metrics. If activated, the "
            "metrics can be accessed via a ROS service.");
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, "",
              "If non-empty, filename of a .pbstream file to load, containing "
              "a saved SLAM state.");
DEFINE_bool(load_frozen_state, true,
            "Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(
    start_trajectory_with_default_topics, true,
    "Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
    save_state_filename, "",
    "If non-empty, serialize state and write it to disk before shutting down.");

namespace cartographer_ros {
namespace {

void Run() {
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  tf2_ros::TransformListener tf(tf_buffer);
  NodeOptions node_options;
  TrajectoryOptions trajectory_options;
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);
    // 获取轨迹配置项指针
  auto trajectory_options_handle = &(trajectory_options);  //
  ros::NodeHandle pnh("~");
  // 获取配置参数
  bool localization = getParam_Func<bool>(pnh, "/localization", 0);      //是否为纯定位模式
  double pos_x = getParam_Func<double>(pnh, "/set_inital_pose_x", 0.0); 
  double pos_y = getParam_Func<double>(pnh, "/set_inital_pose_y", 0.0); 
  double pos_z = getParam_Func<double>(pnh, "/set_inital_pose_z", 0.0); 
  double pos_ox = getParam_Func<double>(pnh, "/set_inital_pose_ox", 0.0); 
  double pos_oy = getParam_Func<double>(pnh, "/set_inital_pose_oy", 0.0); 
  double pos_oz = getParam_Func<double>(pnh, "/set_inital_pose_oz", 0.0); 
  double pos_ow = getParam_Func<double>(pnh, "/set_inital_pose_ow", 1.0); 
  geometry_msgs::Pose init_pose;
  init_pose.position.x = pos_x;
  init_pose.position.y = pos_y;
  init_pose.position.z = pos_z;
  init_pose.orientation.x = pos_ox;
  init_pose.orientation.y = pos_oy;
  init_pose.orientation.z = pos_oz;
  init_pose.orientation.w = pos_ow;

  if(localization == true)
  {
  //更改轨迹配置项中的初始位姿值
  *trajectory_options_handle->trajectory_builder_options.mutable_initial_trajectory_pose()->mutable_relative_pose()
      = cartographer::transform::ToProto(cartographer_ros::ToRigid3d(init_pose));
  }

  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  if (FLAGS_start_trajectory_with_default_topics) {
    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 */);
  }
}

}  // namespace
}  // namespace cartographer_ros

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  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;
  cartographer_ros::Run();
  ::ros::shutdown();
}

​

⑤在cartographer安装目录逐级找到 carto_localization.launch

⑥用VScode打开，代码中增加初始位置设定。第21 22 23分别为x y z坐标，24 25 26 27分别为姿态的四元数。按照第②步查询结果对应写入。

<param name="/localization" type="bool" value = "1"/>
  <param name="/set_inital_pose_x" type="double" value = "-0.14571302361670768"/>
  <param name="/set_inital_pose_y" type="double" value = "1.0819736654414902"/>
  <param name="/set_inital_pose_z" type="double" value = "0.0"/>
  <param name="/set_inital_pose_ox" type="double" value = "0.0"/>
  <param name="/set_inital_pose_oy" type="double" value = "0.0"/>
  <param name="/set_inital_pose_oz" type="double" value = "0.0007467706567228197"/>
  <param name="/set_inital_pose_ow" type="double" value = "0.9999997211667543"/>

完成以上四步，你的机器人就可以在你设定的默认位置启动了，该位置与机器人所在的空间位置一致或在附件。cartographer在此位置搜索位置可以快速定位成功，而无需再全局搜索。

（接下来通过在rviz通过鼠标随意发布位置，将机器人拖动到任意点，实现更加灵活的快速定位，需要多次才能发布完成，点收藏不迷路）