单线激光雷达(Lidar)学习三:使用雷达数据/scan转/PointCloud后生成鸟瞰图
前言:
雷达广泛应用于自动驾驶中,作用非常重要,是自动驾驶无人车中的作为“眼睛“的一环。雷达运行时会产生许多数据,可用于各种各样的计算后使用。下面就使用雷达驱动发布的/scan中的数据进行生成鸟瞰图。(附加代码)
一、 启动雷达节点(以镭神单线雷达ls01b_v2为例进行)
$roslaunch ls01b_v2 ls01b_v2.launch
二、 创建节点订阅/scan并转化为点云信息/PointCloud
详情请参考上一篇博文
/wxhy666/article/details/113674317
三、 启动数据转化节点
$rosrun lswj_pkg data_conversion
(注:lswj_pkg改为自己现用的功能包)
四、 创建节点订阅/PointCloud
#!/usr/bin/env pythonimport syssys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')import cv2sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')from cv_bridge import CvBridgeimport numpy as npimport rospyfrom sensor_msgs.msg import PointCloud2,Imageimport sensor_msgs.point_cloud2 as pc2import osclass pt2brid_eye:def __init__(self): #接收/scan发布/pointcloudself.image_pub = rospy.Publisher('/bird_eyes', Image, queue_size=1)self.pt_sub=rospy.Subscriber("/PointCloud", PointCloud2, self.callback)self.bridge = CvBridge()def callback(self,lidar):lidar = pc2.read_points(lidar)points = np.array(list(lidar))im = self.point_cloud_2_birdseye(points) self.image_pub.publish(self.bridge.cv2_to_imgmsg(im))print("convert!")def point_cloud_2_birdseye(self,points): #鸟瞰图生成x_points = points[:, 0]y_points = points[:, 1]z_points = points[:, 2]f_filt = np.logical_and((x_points > -50), (x_points < 50))s_filt = np.logical_and((y_points > -50), (y_points < 50))filter = np.logical_and(f_filt, s_filt)indices = np.argwhere(filter)x_points = x_points[indices]y_points = y_points[indices]z_points = z_points[indices]x_img = (-y_points*80).astype(np.int32)+500y_img = (-x_points *80).astype(np.int32)+500pixel_values = np.clip(z_points,-2,2)pixel_values = ((pixel_values +2) / 4.0) * 500im=np.zeros([1080,1080],dtype=np.uint8)im[y_img, x_img] = pixel_valuesreturn imif __name__ == '__main__':print("opencv: {}".format(cv2.__version__))pt2img=pt2brid_eye()rospy.init_node('pt_to_brid_eyeImage_node')rospy.spin()
五、 并运行生成鸟瞰图
$rosrun lsw_pkg bird_eye
出现convert则为成功,报错后出现convert则忽略错误即可
六、 开启rviz节点监视/PointCloud点云数据
$rosrun rviz rviz
开启rviz后选择add加入PointCloud2,选择正确的话题topic(PointCloud)
效果如下
七、 开启rqt_image_view查看鸟瞰图
$rosrun rqt_image_view rqt_image_view
话题选择/bird_eyes,效果如下
八、 附带代码
我创建了一个一将运行所有节点的launch文件也添加上去了,有需要可以参考一下
$roslaunch lswj_pkg bird_eye_view.launch
launch 文件内容如下
<launch><node name="ls01b_v2" pkg="ls01b_v2" type="ls01b_v2" output="screen"><param name="scan_topic" value="scan"/> #设置激光数据topic名称<param name="frame_id" value="laser_link"/>#激光坐标<param name="serial_port" value="/dev/ttyUSB0"/> #雷达连接的串口<param name="baud_rate" value="460800" /> #雷达连接的串口波特率<param name="angle_resolution" value="0.25"/>#雷达角度分辨率<param name="zero_as_max" value="false"/> # 设置为true探测不到区域会变成最大值<param name="min_as_zero" value="false"/> # true:探测不到区域为0,false:探测不到区域为inf<param name="rpm" value="600"/> #雷达转速<param name="angle_compensate" value="true"/> #角度补偿,非同轴<rosparam file="$(find ls01b_v2)/launch/includes/filter.yaml" command="load"/></node><node name="data_conversion" pkg="lswj_pkg" type="data_conversion" output="screen"/><node name="bird_eye" pkg="lswj_pkg" type="bird_eye" output="screen"/><node pkg="rviz" type="rviz" name="rviz"/><node pkg="rqt_image_view" type="rqt_image_view" name="rqt_image_view"/></launch>
结语:
本人是一个正在学习的小菜鸟,如果此博文对您有所帮助,请不要忘记点赞哦,谢谢。
仅供参考,如问题诸多,还望指正修改,多多包涵^_<&,