笔者在实践过程中在Slam建图这一块卡了许久,网上教程基本都是复制代码,对初学者很不友好,有的也不正确,笔者肝出来后想提供一篇详细的博客,帮助后来人。 此博客注重原理,使用衫川雷达delta 3i,其他雷达一样的操作原理。
首先了解hector_slam功能包,ros_wiki一波。
1)首先了解话题的发布订阅,需要订阅/scan话题,即激光雷达发出的数据。中间参数可自行了解。 2) 3.1.5提出需要laser 到 base_link的tf变换。 3) 3.1.6提出需要提供map到odom的tf变换。
综上,我们得知,我们需要提供三样东西,则全文围绕提供这三样东西展开。
本人使用衫川雷达,默认为 /dev/ttyUSB0。可使用lsusb以及dmesg | grep ttyS*查看端口号。
首先查找到usb设备的 vid 和 pid,使用lsusb即可查看。例如: 第一个为 vid,第二个为 pid。可通过插拔设备前后对比找到对应设备名称。 找到(没有请自行创建) /etc/udev/rules.d/50-myusb.rules 文件。 添加:
KERNEL=="ttyUSB*", SUBSYSTEMS=="usb", ATTRS{idVendor}=="XXXX", ATTRS{idProduct}=="XXXX", GROUP="你的用户名", MODE="0777",SYMLINK+="修改usb设备名称"例如:
KERNEL=="ttyUSB*", SUBSYSTEMS=="usb", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="rplidar"保存,执行:
sudo service udev reload sudo service udev restart重新插拔USB,输入
ls -l /dev/${刚设置的USB名称}例如
ls -l /dev/rplidar则说明修改成功。 注意:请插入USB设备再执行此命令,否则当然会显示找不到设备。
关于USB权限问题,下面这篇博客介绍的非常清楚。 https://blog.csdn.net/bigdog_1027/article/details/79009603
首先,下载hector_slam功能包,
sudo apt-get install ros-kinetic-hector-slam在你的激光雷达功能包(这个一般厂家会提供,其他常见雷达可在网上搜索教程)中添加hector.launch,配置hector的具体参数。主要修改几个 tf 变换,其他的参数可以不动。
<launch> <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="base_link" /> <param name="base_frame" value="base_link" /> <param name="odom_frame" value="base_link" /> <!-- Tf use --> <param name="use_tf_scan_transformation" value="true"/> <param name="use_tf_pose_start_estimate" value="false"/> <!-- Map size / start point --> <param name="map_resolution" value="0.05"/> <param name="map_size" value="2048"/> <param name="map_start_x" value="0.5"/> <param name="map_start_y" value="0.5" /> <param name="laser_z_min_value" value = "-1.0" /> <param name="laser_z_max_value" value = "1.0" /> <param name="map_multi_res_levels" value="2" /> <param name="map_pub_period" value="2" /> <param name="laser_min_dist" value="0.4" /> <param name="laser_max_dist" value="5.5" /> <param name="output_timing" value="false" /> <param name="pub_map_scanmatch_transform" value="true" /> <!-- Map update parameters --> <param name="update_factor_free" value="0.4"/> <param name="update_factor_occupied" value="0.7" /> <param name="map_update_distance_thresh" value="0.2"/> <param name="map_update_angle_thresh" value="0.06" /> <!-- Advertising config --> <param name="advertise_map_service" value="true"/> <param name="scan_subscriber_queue_size" value="5"/> <param name="scan_topic" value="/scan"/> </node> </launch>我们可以看到以上launch文件已经包含:
<param name="pub_map_odom_transform" value="true"/>这一条代码完成了map到odom的tf变换,ok,完成一项。
下面设置map_frame、base_frame、odom_frame。
<param name="map_frame" value="map" /> <param name="base_frame" value="base_link" /> <param name="odom_frame" value="base_link" />map_frame默认=“map”。 因为hector不需要里程计信息,所以将base_frame、odom_frame都设为base_link。
那么问题来了,我在launch文件里面设置了参数 baseframe=base_link,请问base_link到底是什么?说到底,还是需要我们自己提供urdf文件。
如果仅是学习slam用,可随便选一个urdf模型。博主采用的是古月的mbot_description里的mbot_with_laser。
注意:只要提供机器人模型参数给rviz,打开 rviz 时能正常加载模型即可,如果已有写好的 启动显示模型的 .launch 文件,可跳过此部分。
以下内容为: 将 view_mbot.launch (显示机器人模型参数的 launch 文件)加到 雷达功能包,如果已有现成的,可跳过!
将urdf文件粘贴到你的激光雷达功能包下,并修改: Cmake.list中
find_package(catkin REQUIRED COMPONENTS roscpp rosconsole sensor_msgs urdf xacro )package.xml中:
<build_depend>urdf</build_depend> <build_depend>xacro</build_depend> <run_depend>urdf</run_depend> <run_depend>xacro</run_depend>注意版本不同。 编写(复制)launch文件: view_mbot.launch:
<launch> <param name="robot_description" textfile="$(find delta_lidar)/urdf/mbot_with_laser.urdf" /> <!-- 设置GUI参数,显示关节控制插件 --> <param name="use_gui" value="true"/> <!-- 运行joint_state_publisher节点,发布机器人的关节状态 --> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> <!-- 运行robot_state_publisher节点,发布tf --> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /> <!-- 运行rviz可视化界面 --> <node name="rviz" pkg="rviz" type="rviz" args="-d /opt/ros/kinetic/share/rviz/default.rviz" required="true" /> </launch>运行launch文件,那么现在urdf的参数就到参数服务器了,也就是有了base_link。 下面进行laser到base_link的坐标变换。
这个可在launch文件中,用一行就完成:
<node pkg="tf" type="static_transform_publisher" name="laser_broadcaster" args="0 0 0.105 0 0 0 /base_link /laser_link 100" />加在最后一行即可。
args="0 0 0.105 0 0 0“是laser_link到base_link的变换关系,后面两个参数分别是源坐标系、目标坐标系。
注意:这里的参数应与你的urdf文件里的名称对应! 比如,你的urdf文件里的laser的名称叫”laser“,在launch文件中你写的是”lase_link“,这是不行的。 有的激光雷达的launch文件里面可以设置默认的frame_id: 这里面也要修改成对应的参数。 现在完成了laser_link到base_link的tf变换。
还差/scan话题,当激光雷达节点运行起来后,默认会发布scan话题,如果有不一样,可以先运行起来,再用rostopic list查看具体名称,再到launch文件中修改参数:
<param name="scan_topic" value="/scan"/>到此,hector所需所有要求都已满足,跑起来吧!
最后建图效果:
重点:完成tf变换; /scan 话题
<launch> <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="base_link" /> <param name="base_frame" value="base_link" /> <param name="odom_frame" value="base_link" /> <!-- Tf use --> <param name="use_tf_scan_transformation" value="true"/> <param name="use_tf_pose_start_estimate" value="false"/> <!-- Map size / start point --> <param name="map_resolution" value="0.05"/> <param name="map_size" value="2048"/> <param name="map_start_x" value="0.5"/> <param name="map_start_y" value="0.5" /> <param name="laser_z_min_value" value = "-1.0" /> <param name="laser_z_max_value" value = "1.0" /> <param name="map_multi_res_levels" value="2" /> <param name="map_pub_period" value="2" /> <param name="laser_min_dist" value="0.4" /> <param name="laser_max_dist" value="5.5" /> <param name="output_timing" value="false" /> <param name="pub_map_scanmatch_transform" value="true" /> <!-- Map update parameters --> <param name="update_factor_free" value="0.4"/> <param name="update_factor_occupied" value="0.7" /> <param name="map_update_distance_thresh" value="0.2"/> <param name="map_update_angle_thresh" value="0.06" /> <!-- Advertising config --> <param name="advertise_map_service" value="true"/> <param name="scan_subscriber_queue_size" value="5"/> <param name="scan_topic" value="/scan"/> </node> <node pkg="tf" type="static_transform_publisher" name="laser_broadcaster" args="0 0 0.105 0 0 0 /base_link /laser_link 100" /> </launch>重点:加载机器人模型。
<launch> <param name="robot_description" textfile="$(find delta_lidar)/urdf/mbot_with_laser.urdf" /> <!-- 设置GUI参数,显示关节控制插件 --> <param name="use_gui" value="true"/> <!-- 运行joint_state_publisher节点,发布机器人的关节状态 --> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> <!-- 运行robot_state_publisher节点,发布tf --> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /> <!-- 运行rviz可视化界面 --> <node name="rviz" pkg="rviz" type="rviz" args="-d /opt/ros/kinetic/share/rviz/default.rviz" required="true" /> </launch>希望可以帮到大家!