遇到的問題，目前的毫米波雷達資料很稀疏，用ｅｘｃｅｌ存儲，需要轉換過去，測試一下目前的雷射雷達算法，并測試。
rosbag資料用ｍａｔｌａｂ(linux版本)可以友善的檢視變量和ｔｏｐｉｃ
一. 第一步，用雷射雷達資料測試
[轉載自：https://blog.csdn.net/fk1174/article/details/52673413】
第二種辦法，首先去http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets.php下載下傳資料集，我用的是Intel Research Lab 的資料集，儲存為intel.clf。(clf是一種日志存儲格式)
怎麼下載下傳呢？運作這語句就可以了。
wget http://ais.informatik.uni-freiburg.de/slamevaluation/datasets/aces.clf
編寫把clf檔案轉化為rosbag檔案的python腳本：
#!/usr/bin/env python
'''This is a converter for the Intel Research Lab SLAM dataset
( http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets/intel.clf )
to rosbag'''
import rospy
import rosbag
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from math import pi
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
import tf
def make_tf_msg(x, y, theta, t,base,base0):
trans = TransformStamped()
trans.header.stamp = t
trans.header.frame_id = base
trans.child_frame_id = base0
trans.transform.translation.x = x
trans.transform.translation.y = y
q = tf.transformations.quaternion_from_euler(0, 0, theta)
trans.transform.rotation.x = q[0]
trans.transform.rotation.y = q[1]
trans.transform.rotation.z = q[2]
trans.transform.rotation.w = q[3]
msg = TFMessage()
msg.transforms.append(trans)
return msg
with open('/home/kylefan/catkin_ws/data/intel/intel.clf') as dataset:
with rosbag.Bag('/home/kylefan/intel.bag', 'w') as bag:
for line in dataset.readlines():
line = line.strip()
tokens = line.split(' ')
if len(tokens) <= 2:
continue
if tokens[0] == 'FLASER':
msg = LaserScan()
num_scans = int(tokens[1])
if num_scans != 180 or len(tokens) < num_scans + 9:
rospy.logwarn("unsupported scan format")
continue
msg.header.frame_id = 'base_link'
t = rospy.Time(float(tokens[(num_scans + 8)]))
msg.header.stamp = t
msg.angle_min = -90.0 / 180.0 * pi
msg.angle_max = 90.0 / 180.0 * pi
msg.angle_increment = pi / num_scans
msg.time_increment = 0.2 / 360.0
msg.scan_time = 0.2
msg.range_min = 0.001
msg.range_max = 50.0
msg.ranges = [float(r) for r in tokens[2:(num_scans + 2)]]
bag.write('scan', msg, t)
odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 2):(num_scans + 5)]]
tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t,'base_link','base_laser_link')
bag.write('tf', tf_msg, t)
elif tokens[0] == 'ODOM':
odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
t = rospy.Time(float(tokens[7]))
tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t,'odom','base_link')
bag.write('tf', tf_msg, t)
儲存為bag.py，放到ros包beginner_tutorials的scripts檔案夾下，然後：
chmod +x bag.py
在這之前，需要建立好ｂｅｇｉｎｎｅｒ_tutorialｓ的ｐａｃｋａｇｅ,然後：source ./devel/setup.bash
rosrun beginner_tutorials bag.py
就把雷射和odom的資料按照真實的時間點寫入到了intel.bag裡了。
最後
rosbag play intel.bag
雷射的資料就釋出到/laser了，這樣就實作模拟的作用了。
這時候想再rviz裡看看，記得要在左上角Global Options裡面設定/odom為fixed的：
參考大神：http://answers.ros.org/question/233042/in-ros-gmapping-how-to-use-intel-research-lab-dataset/
具體怎麼運作呢？
Bring up the master: roscore
Make sure that use_sim_time is set to true before any nodes are started: rosparam set use_sim_time true (運作ｒｏｓbag資料就是ｔｒｕｅ,實際資料就是false)
Bring up slam_gmapping, which will take in laser scans (in this case, on the base_scan topic) and produce a map: rosrun gmapping slam_gmapping scan:=scan (我們前面bag.write寫入的就是ｓｃａｎ)
這裡，也可以把ｏｄｏｍ裡程計資料也加進去,就需要這麼寫
rosrun gmapping slam_gmapping scan:=scan _odom_frame:=odom (我們前面bag.write寫入的就是ｓｃａｎ)
In a new terminal, start playing back the bag file to feed data to slam_gmapping: rosbag play --clock
Wait for rosbag to finish and exit.
Save your new map to disk using map_saver from the map_server package: rosrun map_server map_saver -f
遇到的一些問題
０． ros 編譯 Python 檔案
https://blog.csdn.net/light_jiang2016/article/details/55505627
１． 問題與解決
No handlers could be found for logger rosout
解決方法：
在轉換檔案(.py)的開頭加上：
import logging
logging.basicConfig()
其他參考資料
參考網站：http://wiki.ros.org/slam_gmapping/Tutorials/MappingFromLoggedData
https://stackoverflow.com/questions/53642503/how-to-convert-csv-to-rosbag
https://gaoyichao.com/Xiaotu/?book=turtlebot&title=gmapping%E7%9A%84ROS%E5%B0%81%E8%A3%85 GMapping的ROS封裝——初始化
rosnode info /slam_gmapping
----------------------------------------------------
Node [/slam_gmapping]
Publications:
* /map_metadata [nav_msgs/MapMetaData]
* /tf [tf2_msgs/TFMessage]
* /map [nav_msgs/OccupancyGrid]
* /rosout [rosgraph_msgs/Log]
* /slam_gmapping/entropy [std_msgs/Float64]
Subscriptions:
* /tf [tf2_msgs/TFMessage]
* /scan [sensor_msgs/LaserScan]
* /tf_static [tf2_msgs/TFMessage]
* /clock [rosgraph_msgs/Clock]
Services:
* /slam_gmapping/set_logger_level
* /slam_gmapping/get_loggers
* /dynamic_ma