【运行背景】
ROS1 noetic 20.04
安装好Cartographer并能成功运行demo,我的安装办法见以下链接
- Gazebo中启动小车仿真和键盘控制节点
- 启动Cartographer
【Gazebo仿真】
均在spark-slam-gazebo工作空间下
【编写launch文件】
取名为spark_cartographer_slam.launch,内容如下:
<?xml version="1.0"?>
<launch>
<!-- 设置launch文件的参数 -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="true"/>
<arg name="debug" default="false"/>
<arg name="model" default="$(find spark1_description)/urdf/spark1_description_cartographer.urdf"/>
<!-- 运行gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- 运行joint_state_publisher节点,发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<!-- 运行robot_state_publisher节点,发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<!-- 在gazebo中加载机器人模型-->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-unpause -urdf -model marobot1 -param robot_description -x 0.0 -y 0.0"/>
<!--创建新的终端,spark键盘控制 “wsad”分别代表“前后左右”-->
<node pkg="spark_teleop" type="keyboard_control.sh" name="kc_2d" />
</launch>
该launch文件包含以下五个部分:
- 运行Gazebo仿真环境
- 运行joint_state_publisher节点,发布机器人的关节状态
- 运行robot_state_publisher节点,发布tf
- 在gazebo中加载机器人模型
- 运行键盘控制节点
【Cartographer_ros的使用】
均在catkin_google_ws工作空间下
【修改lua文件】
cartographer_ros配置文件.lua文件路径位于cartographer_ros/configuration_files中,选择其中的revo_lds.lua文件复制修改,取新名字为spark_lidar.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",
tracking_frame = "base_footprint",
published_frame = "base_footprint",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
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,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
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.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
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.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
return options
修改了原文件tracking_frame和published_frame参数的值,和使用的机器人匹配
*其他参数值含义可参考官网,链接见以下链接
【修改launch文件】
cartographer_ros的launch文件demo_revo_lds.launch(使用的激光雷达较为相似)路径位于/cartographer_ros/launch中,选择并进行复制修改,取新名字spark_cartographer.launch,内容如下
<?xml version="1.0"?>
<launch>
<arg name="model" default="$(find spark1_description)/urdf/spark1_description_cartographer.urdf"/>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename spark_lidar.lua"
output="screen">
<remap from="scan" to="scan" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
<!-- 加载机器人模型-->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-unpause -urdf -model marobot1 -param robot_description -x 0.0 -y 0.0"/>
</launch>
该launch文件包含四部分:
- 启动cartographer_node节点,配置文件指向刚刚设置的.lua文件,同时映射scan对应的话题
- 启动cartographer_occupancy_grid_node地图格式转换节点,由于cartographer_node建图节点提供的地图是submapList格式的,需要转换成GridMap格式才能在ROS中显示和使用。这里面有两个可配参数,resolution用来设置GridMap地图的分辨率,publish_period_sec用来设置GridMap地图发布的频率。
- 启动Rviz,启动成功后,再根据需求保存配置即可
- 加载机器人模型,方便在rviz上查看,我的模型文件位于另一工作空间
【重新编译】
通过Lua脚本配置参数的方法,每次修改参数后需要重新编译,否则参数无法生效
!!!注意,可能会出现编译失败的情况,这时候将工作空间下的build_isolated,devel_isolated和install_isolated删除后,重新编译即可,如果有更好的办法请告诉我,感谢(后期也会补上对_isolated的学习理解)
【连通工作空间】
【起因】
按照顺序启动gazebo和Cartographer进行建图是没有问题的,只是由于机器人的模型位于spark1_description中,尝试过将文件复制到Cartographer的工作空间中,总是出现编译错误,所以更改环境变量来连通此两工作空间
【修改】
在/catkin_google_ws/devel_isolated下
依次修改cartographer_ros,cartographer_ros_msgs,cartographer_rviz文件夹中的_setup_util.py
找到其中的CMAKE_PREFIX_PATH
添加另一工作空间的devel路径,注意用“ ;”分割
三个文件都修改完后,source一下环境变量
cd catkin_google_ws/
source devel_isolated/setup.bash
用echo $ROS_PACKAGE_PATH查看是否更改成功
看到环境变量已包含两个工作空间,证明成功连通了
【运行仿真Cartographer】
【启动Gazebo仿真】
cd catkin_google_ws/
source devel_isolated/setup.bash
roslaunch spark1_description spark_cartographer_slam.launch
【启动Cartographer】
启动新的终端
cd catkin_google_ws/
source devel_isolated/setup.bash
roslaunch cartographer_ros spark_cartographer.launch
添加围墙和障碍
在启动的Rviz上修改Fixed Frame为odom
修改Views Type为Orbit,拖动视角
控制小车移动建图,建图效果如下
感谢