用速腾16线激光雷达跑gmapping

时间: 2023-07-11 admin IT培训

用速腾16线激光雷达跑gmapping

用速腾16线激光雷达跑gmapping

首先运行激光雷达的驱动,launch文件如下

<launch><arg name="model" default="RS16" /><arg name="device_ip" default="192.168.1.200" /><arg name="msop_port" default="6699" /><arg name="difop_port" default="7788" /><arg name="cut_angle" default="0" doc="If set at [0, 360), cut at specific angle feature activated, otherwise use the fixed packets number mode."/><arg name="lidar_param_path" default="$(find rslidar_pointcloud)/data/rs_lidar_16/"/><node  name="rslidar_node" pkg="rslidar_driver" type="rslidar_node" output="screen" ><param name="model" value="$(arg model)"/><param name="device_ip" value="$(arg device_ip)" /><param name="msop_port" value="$(arg msop_port)" /><param name="difop_port" value="$(arg difop_port)"/><param name="cut_angle" value="$(arg cut_angle)"/><!--param name="pcap" value="path_to_pcap"/--></node><node  name="cloud_node" pkg="rslidar_pointcloud" type="cloud_node" output="screen" ><param name="model" value="$(arg model)"/><param name="curves_path" value="$(arg lidar_param_path)/curves.csv" /><param name="angle_path" value="$(arg lidar_param_path)/angle.csv" /><param name="channel_path" value="$(arg lidar_param_path)/ChannelNum.csv" /><param name="max_distance" value="20"/><param name="min_distance" value="0.4"/><param name="resolution_type" value="0.5cm"/><param name="intensity_mode" value="1"/></node><node name="rviz" pkg="rviz" type="rviz"  args="-d $(find rslidar_pointcloud)/rviz_cfg/rslidar.rviz" /><remap from="/rslidar_points" to="/velodyne_points" /></launch>

然后通过使用pointcloud_to_laserscan包实现三维转二维,launch文件如下

<?xml version="1.0"?><launch><!-- run pointcloud_to_laserscan node --><node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan"><remap from="cloud_in" to="/rslidar_points"/><rosparam># target_frame: rslidar # Leave disabled to output scan in pointcloud frametransform_tolerance: 0.01min_height: -0.4max_height: 1.0angle_min: -3.1415926 # -M_PIangle_max: 3.1415926 # M_PIangle_increment: 0.003 # 0.17degreescan_time: 0.1range_min: 0.2range_max: 100use_inf: trueinf_epsilon: 1.0# Concurrency level, affects number of pointclouds queued for processing and number of threads used# 0 : Detect number of cores# 1 : Single threaded# 2->inf : Parallelism levelconcurrency_level: 1</rosparam></node></launch>

 

运行之后在rviz里添加 laserscan,topic:  /scan,就可以看到白色的边缘线。如下图:

 

然后再通过Laser_scan_matcher功能包虚拟出里程计,并通过demo_gmapping.launch进行直接建图。(ps: 编译这个功能包时把雷达驱动加进去一起编译了)


<launch>#### set up data playback from bag #############################<?ignore <param name="/use_sim_time" value="false"/><!-- 因为Gmapping 的simulation 时间是True, 改为false  网上查到的--->#### rslidar_16 ################################################<!--激光雷达的启动文件-->?>
<arg name="model" default="RS16" /><arg name="device_ip" default="192.168.1.200" /><arg name="msop_port" default="6699" /><arg name="difop_port" default="7788" /><arg name="cut_angle" default="0" doc="If set at [0, 360), cut at specific angle feature activated, otherwise use the fixed packets number mode."/><arg name="lidar_param_path" default="$(find rslidar_pointcloud)/data/rs_lidar_16/"/><node  name="rslidar_node" pkg="rslidar_driver" type="rslidar_node" output="screen" ><param name="model" value="$(arg model)"/><param name="device_ip" value="$(arg device_ip)" /><param name="msop_port" value="$(arg msop_port)" /><param name="difop_port" value="$(arg difop_port)"/><param name="cut_angle" value="$(arg cut_angle)"/><!--param name="pcap" value="path_to_pcap"/--></node>#### publish an example base_link -> laser transform ###########<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" />#### start rviz ################################################<node pkg="rviz" type="rviz" name="rviz" args="-d $(find laser_scan_matcher)/demo/demo_gmapping.rviz"/>#### start the laser scan_matcher ##############################<node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen"><param name="fixed_frame" value = "odom"/><param name="max_iterations" value="10"/><param name="base_frame" value = "base_link"/><param name="use_odom" value="false"/><param name="publy_pose" value = "true"/><param name="publy_tf" value="true"/></node>#### start gmapping ############################################
<!--前三个param必须设置修改,要不然tf_tree不完整--><node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"><param name="base_frame" value="/base_link"/> <!--***机器人的坐标系--><param name="odom_frame" value="/odom" /> <!--***世界坐标系--><param name="map_frame" value="/map" /> <!--***地图坐标系--><param name="map_udpate_interval" value="1.0"/><param name="maxUrange" value="5.0"/><param name="sigma" value="0.1"/><param name="kernelSize" value="1"/><param name="lstep" value="0.15"/><param name="astep" value="0.15"/><param name="iterations" value="1"/><param name="lsigma" value="0.1"/><param name="ogain" value="3.0"/><param name="lskip" value="1"/><param name="srr" value="0.1"/><param name="srt" value="0.2"/><param name="str" value="0.1"/><param name="stt" value="0.2"/><param name="linearUpdate" value="1.0"/><param name="angularUpdate" value="0.5"/><param name="temporalUpdate" value="0.4"/><param name="resampleThreshold" value="0.5"/><param name="particles" value="10"/><param name="xmin" value="-5.0"/><param name="ymin" value="-5.0"/><param name="xmax" value="5.0"/><param name="ymax" value="5.0"/><param name="delta" value="0.02"/><param name="llsamplerange" value="0.01"/><param name="llsamplestep" value="0.05"/><param name="lasamplerange" value="0.05"/><param name="lasamplestep" value="0.05"/></node></launch>

launch文件如下:

 

 

然后就出现了报错

 

 

tf-tree以及 graph如下图:

 

                                                                                                             错误图                                                                                                                                                                                                                                                                                                                                    正确图

错误图

正确图

参考连接如下:

建图

仅用雷达跑gmapping