【运行背景】
ROS1 20.04 noetic
安装好hector
sudo apt install ros-noetic-hector-*
目录
【配置文件】
【仿真环境及机器人启动文件】
编写名为spark_hector_slam_gazebo_rviz.launch的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.urdf"/>
<!-- 运行gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!--<arg name="world_name" value="$(arg world_name)" /> -->
<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>
<!--添加TF转换(map to odom)
<node pkg="tf" type="static_transform_publisher" name="odom_broadcaster" args="0 0 0 0 0 0 1 map odom 50" /> -->
<!-- 在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"/>
<!-- Rviz -->
<arg name="rvizconfig" default="$(find spark1_description)/rviz/gr.rviz" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- SLAM: Hector -->
<include file="$(find spark1_description)/launch/spark_hector.launch">
<!--<remap from="/scan" to="/laser/scan" />
<arg name="configuration_basename" value="$(arg configuration_basename)"/>-->
</include>
<!--创建新的终端,spark键盘控制 “wsad”分别代表“前后左右”-->
<node pkg="spark_teleop" type="keyboard_control.sh" name="kc_2d" />
<!--创建新的终端,确定是否保存地图-->
<node pkg="spark_teleop" type="cmd_save_map.sh" name="csm_2d" />
</launch>
该launch文件启动以下节点:
a)gazebo仿真环境,注意use_sim_time的值
b)joint_state_punlisher,发布机器人关节状态
c)robot_state_publisher,发布TF信息
d)rviz,可视化界面,启动调试后保存配置
e)hector slam的启动配置文件,详细内容会在下文展示
f)键盘控制节点spark_hector.launch
g)map_server,保存地图
【hector配置文件】
编写spark_hector.launch的launch文件配置hector
<launch>
<!-- Arguments -->
<arg name="configuration_basename" default="spark_lds_2d.lua"/>
<arg name="odom_frame" default="odom"/>
<arg name="base_frame" default="base_link"/>
<arg name="scan_subscriber_queue_size" default="5"/>
<arg name="scan_topic" default="scan"/>
<arg name="map_size" default="512"/>
<arg name="pub_map_odom_transform" default="true"/>
<arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
<!-- Hector mapping -->
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- Frame names -->
<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map" />
<param name="odom_frame" value="$(arg odom_frame)" />
<param name="base_frame" value="$(arg base_frame)" />
<!-- Tf use -->
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="false"/>
<!--<param name="pub_map_scanmatch_transform" value="true" />
<param name="pub_map_odom_transform" value="true"/>
<param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />-->
<!-- Map size / start point -->
<param name="map_resolution" value="0.050"/>
<param name="map_size" value="$(arg map_size)"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="map_multi_res_levels" value="2" />
<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.9" />
<param name="map_update_distance_thresh" value="0.1"/>
<param name="map_update_angle_thresh" value="0.04" />
<param name="map_pub_period" value="2" />
<param name="laser_z_min_value" value= "-0.1" />
<param name="laser_z_max_value" value= "0.1" />
<param name="laser_min_dist" value="0.12" />
<param name="laser_max_dist" value="3.5" />
<!-- Advertising config -->
<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
<param name="scan_topic" value="$(arg scan_topic)"/>
<param name="pub_debug_output" value="true"/>
<!-- Debug parameters -->
<!--
<param name="output_timing" value="false"/>
<param name="pub_drawings" value="true"/>
-->
</node>
</launch>
根据机器人实际情况,注意修改odom_frame,base_frame以及scan_topic
【机器人URDF文件】
主要添加雷达传感器,(添加传感器步骤可以看以下文章的如何添加传感器部分)
<link name="virtual_lidar_link" />
<joint name="virtual_lidar_joint"
type="fixed">
<origin
xyz="0 0 0.03"
rpy="0 0 0" />
<parent link="lidar_Link" />
<child link="virtual_lidar_link" />
</joint>
<gazebo reference="virtual_lidar_link">
<sensor type="ray" name="laser_sensor">
<pose>0 0 0.03 0 0 0</pose>
<visualize>true</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>3600</samples>
<resolution>1</resolution>
<min_angle>0</min_angle>
<max_angle>6.283124</max_angle>
</horizontal>
</scan>
<range>
<min>0.042</min>
<max>5</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<!-- plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so" -->
<plugin name="gazebo_ros_spark_laser" filename="libgazebo_ros_laser.so">
<topicName>/scan</topicName>
<frameName>virtual_lidar_link</frameName>
</plugin>
</sensor>
</gazebo>
【运行hector仿真】
启动终端,输入以下命令
roslaunch spark1_description spark_hector_slam_gazebo_rviz.launch
启动仿真环境成功
在gazebo上添加自己搭建的简易模型墙,和障碍物
用键盘控制小车建图,在rviz上显示
使用map_server保存当前地图,生成pgm和yaml文件
查看保存的地图效果
可以看到图像中还是有干扰点的存在,是雷达放置的位置影响了,没有很好的清除干扰点