建图和回环检测|激光SLAMCartographer原理和配置
abstract Welcome to my blog, enter password to read.
Read more
cartographer显示机器人当前位姿|激光SLAMCartographer原理和配置

cartographer发布的 map—->odom 转换频率太低

运行cartographer时，可以发现rviz的报警

1
[SubmapsDisplay::update] line_220  Could not compute submap fading: "map" passed to lookupTransform argument target_frame does not exist.

运行程序carto_pose运行时持续报警:

1
Lookup would require extrapolation into the past.  Requested time 1609143852.338626800 but the earliest data is at time 1609143860.343082666, when looking up transform from frame [base_footprint] to frame [map]

但使用rosrun tf tf_echo map odom看不出问题

查看话题carto_pose，发现位姿更新很慢，甚至时间戳不动，显然是获取坐标系关系不及时。使用rqt_tf_tree发现 map --> odom的频率很低。重新启动cartographer建新的地图，发现发布变换的频率会逐步下降，进而影响tf树的其他变换，因为map坐标系是最顶层
频率很低.png

有时频率又突然到10000，话题carto_pose会接受很多消息。

按定义tf频率的是参数pose_publish_period_sec = 5e-3，但是改了改没有见效。

有时重启后，这个现象没有了，频率正常

显示在map中的位姿

通过tf变换获取位姿坐标，获取的位姿频率只有5hz左右，不满足需求。修改源码，

Node构造函数中添加 current_pose_pub = node_handle_.advertise<geometry_msgs::Pose2D>("current_pose", 10);

在if (trajectory_data.published_to_tracking != nullptr)部分的最后添加

1
2
3
4
5
6
geometry_msgs::Pose2D pos_now;

pos_now.x = static_cast<double>(stamped_transform.transform.translation.x);
pos_now.y = static_cast<double>(stamped_transform.transform.translation.y);
pos_now.theta = tf::getYaw(stamped_transform.transform.rotation);
current_pose_pub.publish(pos_now);

在我的主机上能达到200Hz

如果provide_odom_frame为true，位姿是odom_frame和published_frame之间的关系(但map和odom一般重合)，否则是map_frame和published_frame之间

stamped_transform变量的来源如下

catkin_make_isolated和ninja|cmake/qmake|编译调试

这个命令用于非ROS的包，所以Cartographer用的是它。 缺点是处理pacakge 较慢，只适用于ROS1. 可以和ninja结合

1
catkin_make_isolated  --use-ninja --pkg cartographer_ros

只编译一个包，会真的跳过其他包，比catkin_make --pkg好用多了。 编译生成的文件在/workspace/devel_isolated/cartographer_ros/lib/cartographer_ros

如果加上--install，编译得到的可执行文件在/workspace/devel_isolated/cartographer_ros/install/lib/cartographer_ros


它会在工作空间生成文件夹devel_isolated, install_isolated, build_isolated。

平时我们使用rosrun 运行的是 devel/lib 里的可执行文件，因为一般source的是 catkin_ws/devel/setup.zsh. 修改文件后，使用 catkin_make_isolated 编译，运行发现没有生效，需要先修改为 source catkin_ws/devel_isolated/setup.zsh, 以后就是用catkin_make_isolated

问题

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
 Installing: /home/xiaoqiang/Documents/ros/install_isolated/share/cartographer_ros/launch
-- Installing: /home/xiaoqiang/Documents/ros/install_isolated/share/cartographer_ros/launch/software_institute.launch
-- Up-to-date: /home/xiaoqiang/Documents/ros/install_isolated/share/cartographer_ros/launch/\
CMake Error at cmake_install.cmake:142 (file):
  file INSTALL cannot set permissions on
  "/home/xiaoqiang/Documents/ros/install_isolated/share/cartographer_ros/launch/\"


FAILED: cd /home/xiaoqiang/Documents/ros/build_isolated/cartographer_ros && /usr/bin/cmake -P cmake_install.cmake
ninja: build stopped: subcommand failed.
<== Failed to process package 'cartographer_ros':
  Command '['/home/xiaoqiang/Documents/ros/install_isolated/env.sh', 'ninja', 'install']' returned non-zero exit status 1

Reproduce this error by running:
==> cd /home/xiaoqiang/Documents/ros/build_isolated/cartographer_ros && /home/xiaoqiang/Documents/ros/install_isolated/env.sh ninja install

Command failed, exiting.

不知为何出现了文件夹 install_isolated/share/cartographer_ros/launch/\，删除后重新编译还是如此

参考：
catkin_make_isolated 官方说明
catkin_make vs catkin_make_isolated

(八) Homotopy planner|路径规划TEB算法

实际中用的是Homotopy planner，这里做一些简单说明

算法初始化

构造函数就一句initialize(cfg, obstacles, robot_model, visual, via_points);， 代码比较绕:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
void HomotopyClassPlanner::initialize(const TebConfig& cfg, ObstContainer* obstacles,
          RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, 
          const ViaPointContainer* via_points)
{
  cfg_ = &cfg;
  obstacles_ = obstacles;
  via_points_ = via_points;
  robot_model_ = robot_model;

  if (cfg_->hcp.simple_exploration)
    graph_search_ = boost::shared_ptr<GraphSearchInterface>(new lrKeyPointGraph(*cfg_, this));
  else
    graph_search_ = boost::shared_ptr<GraphSearchInterface>(new ProbRoadmapGraph(*cfg_, this));

  initialized_ = true;
  setVisualization(visual);     // 只有一句，对 visualization_ 赋值
}

simple_exploration默认false，也就是用复杂的ProbRoadmapGraph类。

plan 函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
// initial_plan 其实就是 transformed_plan
bool HomotopyClassPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, 
          const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
  ROS_ASSERT_MSG(initialized_, "Call initialize() first.");

  // store initial plan for further initializations (must be valid for the 
  // lifetime of this object or clearPlanner() is called!)
  initial_plan_ = &initial_plan;

  PoseSE2 start(initial_plan.front().pose);
  PoseSE2 goal(initial_plan.back().pose);

  return plan(start, goal, start_vel, free_goal_vel);
}

// 然后是下面重要的重载函数
bool HomotopyClassPlanner::plan(const PoseSE2& start, const PoseSE2& goal, 
          const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
  ROS_ASSERT_MSG(initialized_, "Call initialize() first.");

  // Update old TEBs with new start, goal and velocity
  updateAllTEBs(&start, &goal, start_vel);

  // Init new TEBs based on newly explored homotopy classes
  exploreEquivalenceClassesAndInitTebs(start, goal, cfg_->obstacles.min_obstacle_dist, start_vel);
  // update via-points if activated
  updateReferenceTrajectoryViaPoints(cfg_->hcp.viapoints_all_candidates);
  // Optimize all trajectories in alternative homotopy classes
  optimizeAllTEBs(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
  // Select which candidate (based on alternative homotopy classes) should be used
  selectBestTeb();

  initial_plan_ = nullptr; // clear pointer to any previous initial plan (any previous plan is useless regarding the h-signature);
  return true;
}

HomotopyClassPlanner::plan —— exploreEquivalenceClassesAndInitTebs —— createGraph() —— PointObstacle::checkLineIntersection —— PointObstacle::checkCollision

1
2
3
4
5
6
7
8
9
PointObstacle::checkLineIntersection
{
  return getMinimumDistance(point) < min_dist;
}

virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
{
      return getMinimumDistance(point) < min_dist;
}

optimizeAllTEBs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
void HomotopyClassPlanner::optimizeAllTEBs(int iter_innerloop, int iter_outerloop)
{
  // optimize TEBs in parallel since they are independend of each other

  if (cfg_->hcp.enable_multithreading)
  {
    // Must prevent .join_all() from throwing exception if interruption was
    // requested, as this can lead to multiple threads operating on the same
    // TEB, which leads to SIGSEGV
    boost::this_thread::disable_interruption di;

    boost::thread_group teb_threads;
    for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
    {
      teb_threads.create_thread( boost::bind(&TebOptimalPlanner::optimizeTEB, 
        it_teb->get(), iter_innerloop, iter_outerloop,  true, 
        cfg_->hcp.selection_obst_cost_scale, cfg_->hcp.selection_viapoint_cost_scale,
        cfg_->hcp.selection_alternative_time_cost)  );
    }
    teb_threads.join_all();
  }
}

EquivalenceClassContainer equivalence_classes_; //!< Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones.

// The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping).

typedef boost::shared_ptr<TebOptimalPlanner> TebOptimalPlannerPtr;

typedef std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer;

teb()函数 返回 const TimedElasticBand&

pbstream_to_ros_map_main.cc 源码解读|激光SLAMCartographer源码解读
abstract Welcome to my blog, enter password to read.
Read more
障碍层(三) purgeStaleObservations|路径规划代价地图

purgeStaleObservations

将timestamp较早的点都移除出observation_list_ 类型std::list<Observation>
唯一调用的地方是bufferCloud

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
if (!observation_list_.empty() )
{
    list<Observation>::iterator obs_it = observation_list_.begin();
    // if keeping observations for no time, only keep one observation

    // observation_keep_time_ 就是参数 observation_persistence
    if (observation_keep_time_ == ros::Duration(0.0))
    {
		  observation_list_.erase(++obs_it, observation_list_.end());
		  return;
    }
    // or loop through the observations to see which ones are stale
    for ( obs_it = observation_list_.begin(); obs_it != observation_list_.end(); 
            ++obs_it )
    {
		Observation& obs = *obs_it;
		// if observation is out of date, remove it and those follow from the list
		// kinetic: ros::Duration time_diff = last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp;
		if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
		{
  			observation_list_.erase(obs_it, observation_list_.end());
  			return;
		}
    }
}

障碍层 (二) 初始化和 pointCloud2Callback|路径规划代价地图

onInitialize

从参数服务器加载参数，并调用matchSize函数，根据主地图的参数来设置障碍层地图。

依据track_unknown_space参数，对代价地图取不同的默认值:

1
2
3
4
if (track_unknown_space)
  default_value_ = NO_INFORMATION;
else
  default_value_ = FREE_SPACE;

expected_update_rate参数

将topics_string记录的观测源逐个输出到source字符串，并找到对应每个观测源的参数。

为每个观测源创建一个buffer，并将其指针存放进observation_buffers_中进行管理。并根据标志位确定是否将其添加到marking_buffers(标记障碍的观测数据) 与 clearing_buffers(用于清除小车与障碍间cost的观测数据):

1
2
3
4
5
6
7
8
9
10
11
12
// create an observation buffer
observation_buffers_.push_back(
  boost::shared_ptr < ObservationBuffer>(new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,  max_obstacle_height, 
      	obstacle_range, raytrace_range,  *tf_,  global_frame_,
          sensor_frame, transform_tolerance) )  );

// check if we'll add this buffer to our marking observation buffers
if (marking)
  	marking_buffers_.push_back(observation_buffers_.back());
// check if we'll also add this buffer to our clearing observation buffers
if (clearing)
  	clearing_buffers_.push_back(observation_buffers_.back());

然后分别针对不同的sensor类型（LaserScan、PointCloud、PointCloud2）注册不同的回调函数，相应的订阅者和filter还要插入相应的容器

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
if (data_type == "LaserScan")
	......
else if (data_type == "PointCloud")
	......
else    // PointCloud2
{
  boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
      > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
  if (inf_is_valid)
     ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
  
  boost::shared_ptr < tf::MessageFilter<sensor_msgs::PointCloud2>
      > filter(new tf::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50)  );
  filter->registerCallback(
      boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back() )  );
  observation_subscribers_.push_back(sub);	  // 传感器数据订阅者
  observation_notifiers_.push_back(filter);   // 保证变换可行
}

ObservationBuffer类是专门用于存储观测数据的类，它是ObstacleLayer的类成员。这里关注一下它的bufferCloud函数

对三种传感器数据，都是先将收到的message转换为sensor_msgs::PointCloud2，再调用ObservationBuffer::bufferCloud，将点云数据存到buffer中，它实际又把点云消息转换为pcl::PointCloud < pcl::PointXYZ >格式后，再调用bufferCloud的重载函数。

这就是一个巨大的缺陷，深度相机和雷达获取的障碍都按PointCloud2类型放到buffer中，对两种障碍只能用同样的costmap参数，比如膨胀半径，影响了导航的调试

ObstacleLayer::pointCloud2Callback

1
2
3
4
5
6
7
void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
                                        const boost::shared_ptr<ObservationBuffer>& buffer)
{
  buffer->lock();
  buffer->bufferCloud(*message);
  buffer->unlock();
}

bufferCloud

非常重要的observation_list_就是在这里赋值的

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud)
{
  Stamped < tf::Vector3 > global_origin;

  // std::list<Observation> observation_list_;
  // create a new observation on the list to be populated
  observation_list_.push_front(Observation());
  
  // check whether the origin frame has been set explicitly or 
  // whether we should get it from the cloud
  // yaml里可以不设置sensor_frame，在这里取点云的坐标系名称
  string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;

  try
  {
    // given these observations come from sensors, we store the origin pt of the sensor
    Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0),
                            pcl_conversions::fromPCL(cloud.header).stamp, origin_frame);

    // global_frame_ 来源是obstacle_layer.cpp 中的 layered_costmap_->getGlobalFrameID();
    // 转换 global_frame_ 和 origin_frame坐标系
    tf_.waitForTransform(global_frame_,  local_origin.frame_id_, 
                         local_origin.stamp_,   ros::Duration(0.5) );
    // local_origin 转换到 global_frame_坐标系，变成global_origin
    tf_.transformPoint(global_frame_, local_origin, global_origin);
    observation_list_.front().origin_.x = global_origin.getX();
    observation_list_.front().origin_.y = global_origin.getY();
    observation_list_.front().origin_.z = global_origin.getZ();

    // pass on the raytrace/obstacle range of the observation buffer to the observations
    observation_list_.front().raytrace_range_ = raytrace_range_;
    observation_list_.front().obstacle_range_ = obstacle_range_;

    pcl::PointCloud < pcl::PointXYZ > global_frame_cloud;

    // 把参数 cloud 转到global_frame_坐标系
    pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_);
    global_frame_cloud.header.stamp = cloud.header.stamp;

    // now we need to remove observations from the cloud that are 
    // below or above our height thresholds

    // 新的变量 observation_cloud，指向observation_list_的成员 cloud_
    pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_);
    unsigned int cloud_size = global_frame_cloud.points.size();
    observation_cloud.points.resize(cloud_size);
    unsigned int point_count = 0;

    // copy over the points that are within our height bounds
    // 从 global_frame_cloud中取出符合 z坐标的点，放到 observation_cloud
    for (unsigned int i = 0; i < cloud_size; ++i)
    {
      if (global_frame_cloud.points[i].z <= max_obstacle_height_
          && global_frame_cloud.points[i].z >= min_obstacle_height_)
      {
        observation_cloud.points[point_count++] = global_frame_cloud.points[i];
      }
    }

    // resize the cloud for the number of legal points
    observation_cloud.points.resize(point_count);
    // 时间戳和坐标系也复制过来
    observation_cloud.header.stamp = cloud.header.stamp;
    observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
  }
  catch (TransformException& ex)
  {
    // if an exception occurs, we need to remove the empty observation from the list
    observation_list_.pop_front();
    ROS_ERROR("TF Exception that should never happen for sensor frame: %s, 
      cloud frame: %s, %s",  sensor_frame_.c_str(),
              cloud.header.frame_id.c_str(),  ex.what());
    return;
  }
  // if the update was successful, we want to update the last updated time
  last_updated_ = ros::Time::now();

  // remove any stale observations from the list
  purgeStaleObservations();
}

服务 clear_costmaps|路径规划代价地图

服务/move_base/clear_costmaps，类型std_srvs/Empty

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
// 定义在 MoveBase构造函数
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

// 回调函数，其实就是两个代价地图重置所有层
bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
{
    //clear the costmaps
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex()));
    controller_costmap_ros_->resetLayers();

    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner( *(planner_costmap_ros_->getCostmap()->getMutex())  );
    planner_costmap_ros_->resetLayers();
    return true;
}

// 主要是 Costmap2D::resetMap 和 Layer::reset()
void Costmap2DROS::resetLayers()
{
	Costmap2D* top = layered_costmap_->getCostmap();
	top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());

	std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
	for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); 
				plugin != plugins->end();  ++plugin)
	{
      (*plugin)->reset();    //每一层覆盖Layer的虚函数reset()
	}
}

先看重置代价地图，这个简单，就是把地图的char值重置为默认值0 FREE_SPACE

1
2
3
4
5
6
7
void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{
	boost::unique_lock<mutex_t> lock(*(access_) );
	unsigned int len = xn - x0;
	for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
		memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
}

reset 所有层

三个层的reset()函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
void InflationLayer::reset() { onInitialize(); }

void ObstacleLayer::reset()
{
    deactivate();
    resetMaps();
    current_ = true;
    activate();
}

void StaticLayer::reset()
{
  if (first_map_only_)
  {
    has_updated_data_ = true;
  }
  else
  {
    onInitialize();
  }
}

这里障碍层的把栅格设置为默认值，取决于参数track_unknown_space，但这个参数一般设置为true，所以栅格的默认值是 NO_INFORMATION

最好不要高频率call clear_costmaps服务。一般是周期性调用，防止代价地图上有太多的garbage

costmap_converter产生的障碍比实际更大|路径规划TEB算法costmap converter

12-16__171652.png

参数cluster_max_distance不能调大，否则线障碍更多，比如这样:
12-17__143426.png

雷达扫描到不存在的障碍|激光SLAM雷达

雷达默认参数

  1. 机器人的杂物挡住了雷达.png
    这是常见的一种情况
  1. 起初经过观察，发现在一些地方容易出现，而且是都在雷达的左侧出现，于是怀疑是雷达自身问题，最后把angle_min的绝对值改小，不再出现这个问题。
    雷达扫描到不存在的障碍

后来又出现这种现象，而且不分左右，有时看不出在哪边
不存在的障碍导致全局路径改变