系列文章目录
·【SLAM】基于explore_lite的移动机器人自主建图
·【SLAM】基于rrt_explore的移动机器人自主建图
·【问题解决】rrt_exploration功能包使用过程中报错处理
前言
上一篇文章介绍了RRT自主建图算法的原理、使用步骤、仿真实验,但是在从ROSWIKI上下载下拉RRT包和例程后,直接编译会产生很多错误,而且这个包官方给出的原话是在Indigo和Kinecit上编译通过,像博主这种系统是ubuntu20.04的noetic系统,一编译就报了很多错。
这篇文章,就用来总结一下报错以及处理方法,让更多小伙伴可以将这个包使用到自己的仿真环境以及真实机器人上。
一、虚拟机仿真
虚拟机环境:ubuntu20.04
1.安装依赖时
在安装RRT的依赖时,需要安装sklearn机器学习库以及numpy,官方给出的安装方式是:
sudo apt-get install python-scikits-learn
sudo apt-get install python-numpy
但是在ubuntu20.04上numpy可以正确安装,而scikits-learn无法正常安装,报错如下:
E: 无法定位软件包 python-scikits-learn
可以使用这行代码,中间可能会显示scipy无法安装,但不影响功能包使用:
pip install scikits-learn
也可以使用python3安装,但是这样会附加安装很多库:
sudo apt-get install python3-scikits-learn
2.TF_REPEATED_DATA ignoring data with redundant timestamp for frame left_wheel at time
开启gazebo并启动rviz时,rviz终端会一直产生一个警告:TF_REPEATED_DATA ignoring data with redundant timestamp for frame left_wheel at time 140.726000 according to authority unknown_publisher
这时可以使用roswtf查看tf坐标变换的错误:
报错提示/robot_state_publisher发布了左右轮到base_link的tf变换,但是/gazebo又发布了左右轮到base_footprint的tf变换,导致了二者的冲突。
解决方法:将机器人的xacro文件中gazebo_control中的publishWheelTF和publishWheelJointState均设置为false就可以解决掉这个问题
3.在ubuntu20.04上rrt_exploration中的simple.launch无法运行
解决方案:
将scripts/assigner.py和scripts/filter.py文件中的开头python改为python3
因为python2和python3对于整数除法运算不同,python2的3/2=1,而python3的3/2=1.5,所以将scripts/functions.py文件进行修改,将"/"改为"//"
4.使用自己搭建的仿真环境时对rrt_exploration的修改
这里我把原来加载机器人模型的部分进行了注释,保留了源代码中给出的move_base文件和rviz配置文件,然后接下来对move_base文件进行修改:
首先,我将这里的激光雷达话题重映射进行了注释,因为我们自己搭建的雷达模型输出话题是/scan。
然后是这里的重映射,给move_base发送的速度控制指令,我们机器人的速度控制话题是/cmd_vel,不需要重映射,应该是作者使用的机器人速度控制指令是/mobile_base/commands/velocity。
最后就是文件最后的几个话题、坐标系需要换成我们搭建的机器人的名字。
另外要注意一点,如果想使用自己的move_base文件,这里需要修改为move_base_node,因为作者在rrt代码中对move_base的命名是这个,否则就需要修改源代码。
最后是rrt_exploration包中的simple.launch,需要将所有的frame_id前面的"/" 都去掉,而所有的topic前面都需要带着"/":
<!-- Launch file for the rrt-detector and the assigner -->
<launch>
<arg name="eta" value="1.0"/>
<arg name="Geta" value="15.0"/>
<param name="namespace_init_count" value="1"/>
<node pkg="rrt_exploration" type="global_rrt_detector" name="global_detector" output="screen">
<param name="eta" value="$(arg Geta)"/>
<param name="map_topic" value="/map"/>
</node>
<node pkg="rrt_exploration" type="local_rrt_detector" name="local_detector" output="screen">
<param name="eta" value="$(arg eta)"/>
<param name="map_topic" value="/map"/>
<param name="robot_frame" value="base_link"/>
</node>
<node pkg="rrt_exploration" type="filter.py" name="filter" output="screen">
<param name="map_topic" value="/map"/>
<param name="info_radius" value="1"/>
<param name="costmap_clearing_threshold" value="70"/>
<param name="goals_topic" value="/detected_points"/>
<param name="namespace" value=""/>
<param name="n_robots" value="1"/>
<param name="rate" value="100"/>
</node>
<node pkg="rrt_exploration" type="assigner.py" name="assigner" output="screen">
<param name="map_topic" value="/map"/>
<param name="global_frame" value="map"/>
<param name="info_radius" value="1"/>
<param name="info_multiplier" value="3.0"/>
<param name="hysteresis_radius" value="3.0"/>
<param name="hysteresis_gain" value="2.0"/>
<param name="frontiers_topic" value="/filtered_points"/>
<param name="n_robots" value="1"/>
<param name="namespace" value=""/>
<param name="delay_after_assignement" value="0.5"/>
<param name="rate" value="100"/>
</node>
</launch>
5.launch文件优化
对于rrt_exploration包中的move_baseSafe.launch也需要进行上述修改,并将几个常用的参数写成了变量arg形式,方便对文件进行修改管理:
<!-- move base -->
<launch>
<master auto="start"/>
<arg name="namespace"/>
<arg name="odom_frame_id" default="odom"/>
<arg name="base_frame_id" default="base_link"/>
<arg name="global_frame_id" default="map"/>
<arg name="laser_frame_id" default="laser_link" />
<arg name="laser_topic" default="/scan" />
<param name="use_sim_time" value="true" />
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" >
<remap from="/scan" to="$(arg laser_topic)"/>
<param name="map_frame" value="$(arg global_frame_id)"/>
<param name="odom_frame" value="$(arg odom_frame_id)"/>
<param name="base_frame" value="$(arg base_frame_id)"/>
<param name="map_update_interval" value="2.0"/>
<param name="maxUrange" value="50.0"/>
<param name="maxRange" value="50.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="linearUpdate" value="0.01"/>
<param name="angularUpdate" value="0.01"/>
<param name="temporalUpdate" value="0.1"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="30"/>
<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.1"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<param name="minimumScore" value="0.005"/>
</node>
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
<param name="footprint_padding" value="0.01" />
<param name="controller_frequency" value="5.0" />
<param name="controller_patience" value="3.0" />
<param name="oscillation_timeout" value="30.0" />
<param name="oscillation_distance" value="0.5" />
<param name="planner_patience" value="1" />
<param name="controller_patience" value="1" />
<remap from="/cmd_vel" to="/cmd_vel"/>
<param name="recovery_behavior_enabled" value="false" />
<rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rrt_exploration_tutorials)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find rrt_exploration_tutorials)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find rrt_exploration_tutorials)/param/base_local_planner_params.yaml" command="load" />
<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="global_costmap/laser_scan_sensor/sensor_frame" value="$(arg laser_frame_id)"/>
<param name="global_costmap/laser_scan_sensor/topic" value="$(arg laser_topic)"/>
<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="local_costmap/laser_scan_sensor/sensor_frame" value="$(arg laser_frame_id)"/>
<param name="local_costmap/laser_scan_sensor/topic" value="$(arg laser_topic)"/>
<param name="local_costmap/obstacle_layer/laser_scan_sensor/topic" value="$(arg laser_topic)"/>
</node>
</launch>
6.打点完成后机器人不运动
检查move_base配置文件中的底盘坐标系是base_link还是base_footprint,这里应该设置为你的机器人的底盘坐标系(随机器人移动的)
二、真实机器人
暂时没测试,回学校测试后完成