cartographer_grpc_server的使用和配置

使用gRPC与云端结合:cartographer使用Protobuf数据传输协议,所以可以使用gRPC(也是谷歌的),可以通过云端运行cartographer算法,最典型的例子就是多机器人在一张已知地图上进行导航,在一个远程的强大centralized localization server运行 multi-trajectories Cartographer

方案:一台机器运行local optimization,另一台运行 global optimization

Cloud-based mapping ,using a gRPC streaming connection between local and global SLAM to propagate sensor data and local SLAM updates, which does not support ACKs and resending, hence this is not going to work over spotty WiFi.

先安装protobufceres

安装 grpc

一开始下载grpc后进行编译,结果报错:

1
2
3
4
CMake Warning at cmake/abseil-cpp.cmake:30 (message):
gRPC_ABSL_PROVIDER is "module" but ABSL_ROOT_DIR is wrong
Call Stack (most recent call first):
CMakeLists.txt:188 (include)

这说明第三方库没准备好,最好按如下步骤:

如果只想下载指定版本的,如以版本1.27.3为例,可改成如下语句:

1
git clone -b v1.27.0 https://github.com/grpc/grpc.git

上列操作成功完成后,gRPC 源码的第三方依赖目录 third_party 实际是空的,需通过下列步骤拉取依赖的第三方。切换到 grpc 目录,下载 grpc 第三方依赖到本地:
1
git submodule update --init

grpc依赖的第三方库有点多,如果不借助git submodule,手工一个个下载不太容易。其中的bloaty库很大。

注意,gRPC 要求 CMake 3.5.1 或以上版本的CMake,否则会报错 CMake 3.5.1 or higher is required

编译:

1
2
3
4
5
6
7
8
9
10
11
cmake -DgRPC_SSL_PROVIDER=package ..
make -j8
sudo make install

cmake -DgRPC_INSTALL=ON -DgRPC_ZLIB_PROVIDER=package -DgRPC_CARES_PROVIDER=package -DgRPC_PROTOBUF_PROVIDER=package -DgRPC_SSL_PROVIDER=package -DgRPC_ABSL_PROVIDER=package ..
make -j8
sudo make install

cmake -DgRPC_INSTALL=ON -DBUILD_SHARED_LIBS=ON -DgRPC_ZLIB_PROVIDER=package -DgRPC_CARES_PROVIDER=package -DgRPC_PROTOBUF_PROVIDER=package -DgRPC_SSL_PROVIDER=package -DgRPC_ABSL_PROVIDER=package ..
make -j8
sudo make install

CMake配置

在包含option(BUILD_GRPC "build Cartographer gRPC support" false)的3个CMakeLists修改为true。 最好再添加 set(BUILD_GRPC true),以及C++14的支持 set(CMAKE_CXX_STANDARD 14)

lua配置

backpack_2d_server.lua如下:

1
2
3
4
include "map_builder_server.lua"

MAP_BUILDER_SERVER.map_builder.use_trajectory_builder_2d = true
return MAP_BUILDER_SERVER

map_builder_server.lua如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
include "map_builder.lua"

MAP_BUILDER_SERVER = {
map_builder = MAP_BUILDER,
num_event_threads = 4,
num_grpc_threads = 4,
--- 我的修改
server_address = "192.168.0.105:50051",
uplink_server_address = "",
upload_batch_size = 100,
enable_ssl_encryption = false,
enable_google_auth = false,
}

运行数据集

1
roslaunch cartographer_ros grpc_demo_backpack_2d.launch bag_filename:=/home/user/cartographer/dataset/cartographer_paper_deutsches_museum.bag

这是在一台机上运行客户端和服务端

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
<launch>
<param name="/use_sim_time" value="true" />
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />

<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<!-- 服务端 -->
<node name="cartographer_grpc_server" pkg="cartographer_ros"
type="cartographer_grpc_server.sh" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_server.lua">
</node>
<!-- 客户端 -->
<node name="cartographer_grpc_node" pkg="cartographer_ros"
type="cartographer_grpc_node" args="
-client_id CLIENT_ID
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d.lua"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />
</node>

<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

就是CLIENT_ID,无需设置为数字。

真实运行

  • 把客户端上的ROS_MASTER_URI设置为服务端的IP
  • rviz放在服务端。
  • 服务端的终端刷新日志,客户端不刷新
  • 网络不能差,否则服务端刷新慢。
  • 连接成功后,关服务端,客户端也会终止。连接成功后,关客户端,服务端停止刷新;再启动客户端,服务端继续更新。

客户端

1
2
3
4
5
6
7
8
9
10
11
12
<launch>
<node name="cartographer_grpc_node" pkg="cartographer_ros"
type="cartographer_grpc_node" args="
-client_id CLIENT_ID
-server_address 192.168.1.213:50051
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d.lua"
output="screen">
<remap from="/scan" to="/sick_tim551_scan" />
<remap from="/odom" to="/odometry/filtered" />
</node>
</launch>

服务端

1
2
3
4
5
6
7
8
9
10
<launch>
<node name="cartographer_grpc_server" pkg="cartographer_ros"
type="cartographer_grpc_server.sh" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_server.lua">
</node>

<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

纯定位

1
2
3
4
5
6
7
8
9
<node name="cartographer_grpc_node" pkg="cartographer_ros"
type="cartographer_grpc_node" args="
-client_id CLIENT_ID
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_localization.lua
-load_state_filename $(arg load_state_filename)"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />
</node>

参考:
运行cartographer的gRPC demo
Ubuntu下安装c-ares库