本文已参与「新人创作礼」活动，一起开启掘金创作之路。
一、录制地图包
1.1 运行机器人小车节点
roslaunch lp_robot lp_robot.launch
1.2 启动小车控制节点
roslaunch lp_robot keybord.launch
1.3 rosbag记录数据
# 记录全部话题
rosbag record -a
或者
# 记录部分话题
rosbag record /scan /imu /odom
1.4 播放录制的bag（查看是否录制成功）
rosbag play XXX.bag
查看话题：rostopic list 查看节点：rosnode list 打印话题内容：rostopic echo /scan /odom
将本机的map.pgm发送到[email protected]:/home/lp_hy/bag/zk的文件夹中，在本机终端运行命令。 scp -r map.pgm [email protected]:/home/lp_hy/bag/zk
二、生成pbstrem文件
录制完地图包之后，需要将.bag转化成.pbstream文件，命令如下：
roslaunch cartographer_ros offline_backpack_2d_lp_hy.launch bag_filenames:=/home/lyp/bag/zz/XXXX.bag
在转化的过程中会启动rviz，可以看到机器人小车的运动轨迹。所以尽量在本地电脑上运行。
三、pbstream转化成pgm和yaml
将.pbstream文件转化成.pgm和.yaml文件，命令如下：
rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=./map -pbstream_filename=/home/lyp/zhankun/cartographer_bag/cartographer_paper_deutsches_museum.bag.pbstream -resolution=0.05
在得到.pgm和.yaml文件之后，还需要对.pgm文件进行后处理，将扫描的边界障碍物重新用黑色笔描一遍。
==对于二、三步骤，常使用以下方案：==
- 使用
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=XXX.bag；会播放建图过程- 播放完毕，使用
rosservice call /finish_trajectory 0命令，完成轨迹, 不接受进一步的数据- 使用
rosservice call /write_state "{filename: '${HOME}/Downloads/mymap.pbstream'}"序列化保存其当前状态，保存成pbstream文件- 使用
rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=${HOME}/Downloads/mymap -pbstream_filename=${HOME}/Downloads/mymap.pbstream -resolution=0.05生成pgm和yaml文件。注意yaml文件中image的路径。。。。。
四、功能1：键盘控制小车运动
如果只是想用键盘控制小车的运动，可以启动keybord.launch文件，命令如下：（需要启动小车和键盘节点）
roslaunch lp_robot lp_robot.launch
roslaunch lp_robot keybord.launch
最好在机器人小车的系统终端上运行命令：ssh [email protected] wsad分别控制小车的前进、后退、左转、右转。WSAD对应小车快速运动命令。
五、功能2：建图定位与导航
如果想要自动导航，依次打开终端窗口运行以下命令：
5.1 运行激光雷达、小车底盘等节点
roslaunch lp_robot lp_robot.launch
5.2 运行move_base节点
roslaunch lp_robot move_base.launch
5.3 运行carto节点
roslaunch cartographer_ros backpack_2d_location_lp.launch
5.4 运行Navigation/mapserver节点
需要将pbstream转化成的pgm和yaml文件导入到机器人小车里面，然后再运行以下命令：
rosrun map_server map_server XXXX.pgm.yaml
5.5 运行rviz显示
rviz
前4个命令最好在机器人小车系统终端中运行，最后的rviz在本地电脑终端运行。 运行命令之前，打开终端要记得source一下。
六、附件1：相关配置文件
6.1 Carto的launch文件——offline_backpack_2d_lp_hy.launch
在将bag转化成.pbtream文件会用到此/home/lyp/project/carto_ws/install_isolated/share/cartographer_ros/launch/offline_backpack_2d_lp_hy.launch文件
<!--
Copyright 2018 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.
-->
<launch>
<arg name="bag_filenames"/>
<arg name="no_rviz" default="false"/>
<arg name="rviz_config" default="$(find cartographer_ros)/configuration_files/demo_2d.rviz"/>
<arg name="configuration_directory" default="$(find cartographer_ros)/configuration_files"/>
<arg name="configuration_basenames" default="backpack_2d_lp_hy.lua"/>
<arg name="urdf_filenames" default="$(find cartographer_ros)/urdf/backpack_2d_lp_hy.urdf"/>
<arg name="launch_prefix" default=""/>
<include file="$(find cartographer_ros)/launch/offline_node_lp.launch">
<arg name="bag_filenames" value="$(arg bag_filenames)"/>
<arg name="no_rviz" value="$(arg no_rviz)"/>
<arg name="rviz_config" value="$(arg rviz_config)"/>
<arg name="configuration_directory" value="$(arg configuration_directory)"/>
<arg name="configuration_basenames" value="$(arg configuration_basenames)"/>
<arg name="urdf_filenames" value="$(arg urdf_filenames)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
</include>
</launch>
6.2 配置文件——backpack_2d_lp_hy.lua
/home/lyp/project/carto_ws/install_isolated/share/cartographer_ros/configuration_files/backpack_2d_lp_hy.lua
-- 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 "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map", --用于发布submap的ROS坐标系ID，位姿的父坐标系，通常使用“map”
tracking_frame = "base_link", --由SLAM算法追踪的ROS坐标系ID，如果使用IMU，应该使用“imu_link”
published_frame = "base_link", --用于发布位姿子坐标系的ROS坐标系ID
odom_frame = "odom", --在provide_odom_frame为真才启用，坐标系在published_frame和map_frame之间用于发布局部SLAM结果，通常是“odom”
provide_odom_frame = true, --如果启用，局部，非闭环，持续位姿会作为odom_frame发布在map_frame中发布。
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = true, --如果启用，订阅关于“odom”话题的nav_msgs/Odometry消息。里程信息会提供，这些信息包含在SLAM里
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1, --订阅的激光扫描话题数量。
num_multi_echo_laser_scans = 0, --订阅的多回波激光扫描主题的数量。
num_subdivisions_per_laser_scan = 1, -- 将每个接收到的（多回波）激光扫描分成的点云数
num_point_clouds = 0, --要订阅的点云话题的数量。
lookup_transform_timeout_sec = 0.2, --使用tf2查找变换的超时秒数。
submap_publish_period_sec = 0.3, --发布submap的间隔（以秒为单位），例如， 0.3秒
pose_publish_period_sec = 5e-3, --发布姿势的间隔（以秒为单位），例如 5e-3，频率为200 Hz。
trajectory_publish_period_sec = 30e-3, --以秒为单位发布轨迹标记的时间间隔，例如， 30e-3持续30毫秒。
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.use_imu_data=false
TRAJECTORY_BUILDER_2D.min_range = 0.1
TRAJECTORY_BUILDER_2D.max_range =50
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 50
TRAJECTORY_BUILDER_2D.num_accumulated_range_data =1
TRAJECTORY_BUILDER_2D.voxel_filter_size =0.01
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window =0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window =math.rad(20.)
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight =1
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight=10.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight=10.
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds=5
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters =0.2
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians=math.rad(180.)
TRAJECTORY_BUILDER_2D.submaps.num_range_data=120
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability= 0.6
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability =0.4
POSE_GRAPH.optimize_every_n_nodes =30
POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.global_constraint_search_after_n_seconds=5
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.40
-- fast localization
MAP_BUILDER.num_background_threads = 8
POSE_GRAPH.constraint_builder.sampling_ratio = 0.5
POSE_GRAPH.global_sampling_ratio =0.001
POSE_GRAPH.constraint_builder.log_matches =true
return options
6.3 描述文件——backpack_2d_lp_hy.urdf
/home/lyp/project/carto_ws/install_isolated/share/cartographer_ros/urdf/backpack_2d_lp_hy.urdf
<!--
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.
-->
<robot name="cartographer_backpack_2d">
<material name="orange">
<color rgba="1.0 0.5 0.2 1" />
</material>
<material name="gray">
<color rgba="0.2 0.2 0.2 1" />
</material>
<link name="imu_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<box size="0.06 0.04 0.02" />
</geometry>
<material name="orange" />
</visual>
</link>
<link name="horizontal_laser_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.03" />
</geometry>
<material name="gray" />
</visual>
</link>
<link name="vertical_laser_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.03" />
</geometry>
<material name="gray" />
</visual>
</link>
<link name="base_link" />
<link name="laser" />
<joint name="imu_link_joint" type="fixed">
<parent link="base_link" />
<child link="imu_link" />
<origin xyz="0 0 0" />
</joint>
<joint name="horizontal_laser_link_joint_10" type="fixed">
<parent link="base_link" />
<child link="laser" />
<origin xyz="0 0 0.2" rpy="0 0 0"/>
</joint>
<joint name="horizontal_laser_link_joint" type="fixed">
<parent link="base_link" />
<child link="horizontal_laser_link" />
<origin xyz="0.1007 0 0.0558" />
</joint>
<joint name="vertical_laser_link_joint" type="fixed">
<parent link="base_link" />
<child link="vertical_laser_link" />
<origin rpy="0 -1.570796 3.141593" xyz="0.1007 0 0.1814" />
</joint>
</robot>
七、附件2：重要文件备份
7.1 lp_robot.launch
/home/lp_hy/catkin_ws/src/lp_robot/launch/lp_robot.launch
<launch>
<arg name="cmd_topic" default="cmd_vel" />
<arg name="walk_vel" default="0.4" />
<arg name="run_vel" default="0.8" />
<arg name="yaw_rate" default="0.2" />
<arg name="yaw_rate_run" default="0.4" />
<arg name="hz" default="50" />
<!--
<include file= "$(find realsense2_camera)/launch/rs_camera.launch"/>
<include file="$(find lslidar_c16_decoder)/launch/lslidar_c16.launch"/>
-->
<include file="$(find sick_scan)/launch/sick_lms_1xx.launch"/>
<!--
<include file="$(find cartograper_ros)/launch/backpack_2d_lp.launch"/>
<include file="$(find lp_robot)/launch/move_base.launch"/>
<node pkg="lp_robot" type="lp_robot_keybord" name="lp_robot">
<remap from="cmd_vel" to="$(arg cmd_topic)" />
<param name="hz" value="$(arg hz)"/>
<param name="walk_vel" value="$(arg walk_vel)"/>
<param name="run_vel" value="$(arg run_vel)"/>
<param name="yaw_rate" value="$(arg yaw_rate)"/>
<param name="yaw_rate_run" value="$(arg yaw_rate_run)"/>
</node>
-->
<node pkg="lp_robot" type="lp_robot_hw_node" name="lp_robot_hw_node" output="screen"/>
</launch>
7.2 keybord.launch
/home/lp_hy/catkin_ws/src/lp_robot/launch/keybord.launch
<launch>
<arg name="cmd_topic" default="cmd_vel" />
<arg name="walk_vel" default="0.05" />
<arg name="run_vel" default="0.4" />
<arg name="yaw_rate" default="0.4" />
<arg name="yaw_rate_run" default="0.8" />
<arg name="hz" default="50" />
<node pkg="lp_robot" type="lp_robot_keybord" name="lp_robot">
<remap from="cmd_vel" to="$(arg cmd_topic)" />
<param name="hz" value="$(arg hz)"/>
<param name="walk_vel" value="$(arg walk_vel)"/>
<param name="run_vel" value="$(arg run_vel)"/>
<param name="yaw_rate" value="$(arg yaw_rate)"/>
<param name="yaw_rate_run" value="$(arg yaw_rate_run)"/>
</node>
</launch>
7.3 move_base.launch
/home/lp_hy/catkin_ws/src/lp_robot/launch/move_base.launch
<!--
ROS navigation stack with velocity smoother and safety (reactive) controller
-->
<launch>
<arg name="odom_frame_id" default="map"/>
<arg name="base_frame_id" default="base_link"/>
<arg name="global_frame_id" default="map"/>
<arg name="odom_topic" default="/odom" />
<arg name="laser_topic" default="/scan" />
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find lp_robot)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find lp_robot)/param/costmap_common_params_local.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find lp_robot)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find lp_robot)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find lp_robot)/param/teb_local_planner_params.yaml" command="load" />
<rosparam file="$(find lp_robot)/param/global_planner_params.yaml" command="load" />
<!-- reset frame_id parameters using user input data -->
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="planner_frequency" value="0.0" />
<param name="planner_patience" value="5.0" />
<!-- param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" /-->
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /><param name="controller_frequency" value="10.0" />
<param name="controller_patience" value="5.0" />
<param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
<param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
<param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
<param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
<param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>
<!-- <remap from="cmd_vel" to="raw_cmd_vel"/> -->
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="scan" to="$(arg laser_topic)"/>
</node>
</launch>
7.4 backpack_2d_location_lp.launch
/home/lp_hy/carto_ws/install_isolated/share/cartographer_ros/launch/backpack_2d_location_lp.launch
<!--
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.
-save_state_filename= /home/mini/meeting.pbstream"
-load_state_filename /home/mini/meeting.pbstream"
-->
<launch>
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d_lp.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_location_lp.lua
-load_state_filename /home/lp_hy/bag/2021-01-18-10-01-32.bag.pbstream"
output="screen">
<remap from="/imu" to="/camera/imu" />
<remap from="/scan" to="/scan" />
</node>
</launch>
八、实车测试流程
第一步：修改ip和hosts
首先，运行以下命令：
sudo vim ~/.bashrc
添加IP：
export ROS_HOSTNAME=192.168.100.204
export ROS_MASTER_URI=http://192.168.100.186:11311
然后，运行以下命令：
sudo vim /etc/hosts
添加IP和用户名：
192.168.100.186 lp_hy
第二步：远程登陆机器人小车
ssh [email protected]
第三步：实现功能1
运行第四节的命令，实现手动控制。
第四步：实现功能2
首先录包，然后转化成pbstream,接着转化成pgm和yaml，将pgm和yaml拷贝到机器人小车中，最后运行第五节的命令，实现自动控制。运行完各节点和rviz之后，需要在rviz中添加话题。/map、/costmap（global）、/costmap（local）、TF等等。
点击rviz中的2D Nav Goal，然后在地图中选中点，等待机器人小车自动规划路径，自主导航。
如果单独查看pointcloud2点云，Fixed Frame需要修改成laser。