linux毫米波雷达程序,用毫米波雷达数据做ＳＬＡＭ

遇到的问题，目前的毫米波雷达数据很稀疏，用ｅｘｃｅｌ存储，需要转换过去，测试一下当前的激光雷达算法，并测试。

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