三台车使用RRT和gmapping联合建图

• 集群机器人可以协作进行地图构建吗？
  两个问题：每个机器人的定位；每个机器人和主节点通信。机器人1把扫描的点加到地图，机器人2把扫描的点加到地图，最后形成完整地图。

• 室内的两个车怎么进行准确定位呢？
  环境不大的话，在天花板上贴二维码，两个机器人都去识别二维码。如果环境很大，初始建图时候，让两个机器人离得近一些，手工量出来二者的相对位置，建图时候在增量定位，这样也可以定位

同事的点云程序必须用PCL-1.8，而ROS默认的是1.7，结果有时出现了版本冲突。

安装了PCL-1.8后，重要的库文件libcostmap_2d.so链接到了pcl-1.8，而不是默认的1.7。此时删除了pcl-1.8之后，再编译会报错:

1
2
3
4

CMake Error at /home/user/catkin_ws/devel/share/costmap_2d/cmake/costmap_2dConfig.cmake:113 (message):
Project 'costmap_2d' specifies '/usr/local/include/pcl-1.8' as an include
dir, which is not found.  It does neither exist as an absolute directory
nor in '/home/user/catkin_ws/src/navigation/costmap_2d//usr/local/include/pcl-1.8'.

我目前的机器人发布tag_detections话题的频率只有0.6Hz，只能换另一台高配置的机器人，频率15Hz。如果机器人走得慢，不必很高。

landmark的观测是 tracking frame，一般是imu或者base_footprint。这个在SensorBridge::HandleLandmarkMessage里已经处理了。

如果关闭后端，landmark机制就无法触发了。

我直接修改apriltag_ros的continuous_detector.cpp中的imageCallback，如果再写一个程序订阅再发布新话题，会增加网络损耗。相机检测到二维码时， 最好有一定的位移或旋转， 这样才能触发landmark机制，rviz里会用MarkerArray标出landmark的位置

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

markers: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1611712040
        nsecs:  13247643
      frame_id: "map"
    ns: "Landmarks"
    id: 0
    type: 2
    action: 0
    pose: 
      position: 
        x: 0.0142477289141
        y: 0.140619658508
        z: 0.337341305453
      orientation: 
        x: 0.987908805705
        y: 0.0191071294953
        z: 0.015915594168
        w: -0.153028765515
    scale: 
      x: 0.2
      y: 0.2
      z: 0.2
    color: 
      r: 0.207129895687
      g: 0.115499980748
      b: 0.769999980927
      a: 1.0
    lifetime: 
      secs: 0
      nsecs:         0
    frame_locked: False
    points: []
    colors: []
    text: ''
    mesh_resource: ''
    mesh_use_embedded_materials: False

参考:
知乎-带Landmark的demo运行和测试
Cartographer——带Landmark的demo的运行和测试

明明安装了库，也在CMake里指定了路径，但还是报这个错误

1. 修改/etc/ld.so.conf 然后刷新

Linux回到这个文件定义的路径里寻找库文件

1
2
3

sudo vim /etc/ld.so.conf
# 添加你的库文件路径
sudo ldconfig

1. 修改环境变量LD_LIBRARY_PATH

1
2

export LD_LIBRARY_PATH=/where/you/install/lib:$LD_LIBRARY_PATH
sudo ldconfig

1. 用ln将需要的so文件链接到/usr/lib或者/lib这两个默认的目录下边

1
2

ln -s /where/you/install/lib/*.so /usr/lib
sudo ldconfig

1. 强行将路径写进去
2. 路径写进去不行的话，在~/.bashrc中，导入export环境变量 LD_LIBRARY_PATH
3. 在/etc/ld.so.conf/目录下，添加该文件的路径，首先命令行输入locate package，定位软件包的位置，然后找个.conf文件，将路径加进去之后，重新sudo ldconfig

运行时动态库的搜索路径的先后顺序是

1. 编译目标代码时指定的动态库搜索路径
2. 环境变量LD_LIBRARY_PATH指定的动态库搜索路径
3. 配置文件/etc/ld.so.conf中指定的动态库搜索路径
4. 默认的动态库搜索路径/lib和/usr/lib

这个顺序是compile gcc时写在程序内的，通常软件源代码自带的动态库不会太多，而我们的/lib和/usr/lib只有root权限才可以修改，如果对环境变量LD_LIBRARY_PATH进行操作就能解决问题，不要改/etc/ld.so.conf，/lib和/usr/lib

设置定位中的初始位姿

使用cartographer的定位模式时， 机器人的初始位姿默认为建图时的起点位姿，机器人运动一段时间后，通过新局部子图与全局地图相匹配，以获取较为理想的位姿，故无需考虑机器人启动位姿，这个定位过程可以在rviz中观察到。

如果不在初始位姿附近，有可能不能定位成功，所以设置启动时的初始位姿，可以加速定位。 初始位姿不能像AMCL那样在Rviz里用箭头给定

定位模式载入的地图在submap中的trajectory id为0。开始定位后，系统默认从起点开始建立子图进行匹配，此时，该子图在submap中的trajectory id为1。要使用start_trajectory服务，必须先终止旧的trajectory为1的建图，调用rosservice call /finish_trajectory， cartographer_ros_msgs/srv/FinishTrajectory.srv如下：

1
2
3

int32 trajectory_id
---
cartographer_ros_msgs/StatusResponse status

然后再开始设定的初始位姿的建图，调用rosservice call /start_trajectory，cartographer_ros_msgs/srv/StartTrajectory.srv如下：

1
2
3
4
5

cartographer_ros_msgs/TrajectoryOptions options
cartographer_ros_msgs/SensorTopics topics
---
cartographer_ros_msgs/StatusResponse status
int32 trajectory_id

更方便的方法是修改源码cartographer_ros/start_trajectory_main.cc，加入#include"cartographer_ros_msgs/FinishTrajectory.h"，在Run()函数开头部分中加入：

1
2
3
4
5
6
7
8
9
10

ros::Duration(0.3).sleep();
ros::NodeHandle node_handle;

ros::ServiceClient finish_trajiectry_client =
    node_handle.serviceClient<cartographer_ros_msgs::FinishTrajectory>(
        kFinishTrajectoryServiceName);

cartographer_ros_msgs::FinishTrajectory f_srv;
f_srv.request.trajectory_id = 1;
finish_trajiectry_client.call(f_srv);

编译完成后，在home_no_odom_localization.launch中添加：

1
2
3
4
5

<node name="cartographer_start_trajectory" pkg="cartographer_ros" type="cartographer_start_trajectory"
      args= "
      -configuration_directory $(find cartographer_ros)/configuration_files
      -configuration_basename home_localization.lua
      -initial_pose {to_trajectory_id=0,relative_pose={translation={0.27,1.12,-0.146},rotation={0,0,1.486,timestamp=0}}}"/>

重点是initial_pose部分： 位姿 is relative to trajectory 0的原点，这是参数to_trajectory_id的作用。ID只能是地图当前所用的，一般是0，最好用/get_trajectory_states服务检查；

relative_pose就是要指定的位姿；timestamp=0非常重要，如果不写这个timestamp，cartographer会默认为当前的时间，结果传入的pose会根据时间做一个位姿变化；rotation是欧拉角（弧度）。

机器人将以initial_pose为起始位姿开辟一条新的trajectory，其id为2。 查看trajectory_node_list话题的ns属性，获知当前运行的trajectory_id。 把机器人开到初始位姿附近，启动launch之后，很快就能定位成功。如果机器人启动的位姿离给定的太远，那么定位过程跟从原点寻找差不多

对于新版本的cartographer_ros，执行

1

rosservice call /start_trajectory "configuration_directory: '' configuration_basename: '' use_initial_pose: false initial_pose: position: {x: 0.0, y: 0.0, z: 0.0} orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0} relative_to_trajectory_id: 0"

小强机器人使用 cartographer 在2条trajectory下的定位

要加速定位，需要调整参数POSE_GRAPH.constraint_builder.sampling_ratio， 增大POSE_GRAPH.global_sampling_ratio

纯定位的过程

补充

目前从SLAM里判断是否定位成功是不可能的。

一次纯定位时，无论怎么调整参数都失败，重启机器人后正常了。

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]

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

有时频率又突然到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);

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

stamped_transform变量的来源如下

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

1

catkin_make_isolated  --use-ninja --pkg cartographer_ros

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

它会在工作空间生成文件夹devel_isolated, install_isolated, build_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