前言:基于2D激光雷达的机器人,想让它跑自动导航,众所周知有2个比较明显的缺陷,1,那就是普通的激光雷达无法对玻璃或是镜面物体有反映; 2,机器人避障时只能对某一个平面的物体有反映,超过或者低于这个平面就不行,类似桌面等悬空的物体就无法检测。

基于这个缺陷,大部分的做法是使用廉价的超声雷达来辅助激光雷达,达到改善这个缺陷的目地。而且超声波模块很便宜,大部分在5~15元之间。测距误差在<3cm,用于机器人效果上还是不错的。之前的做法是,直接读取超声然后判断障碍物是否在安全距离,在安全距离以内就急停,这种做法有一个缺陷,那就是无法让机器人对障碍物进行绕行,所有把超声的数据安装PCL点云的方式发布出去,然后塞到局部动态地图里面,这样就可以达到使得机器人可以向激光雷达一样,做绕行的动作。废话少说,直接上代码:

class TestPcl():
    def __init__(self):
        rospy.init_node('talk_sonar_pcl', anonymous=True)
        pub_cloud = rospy.Publisher("sonar_pcl", PointCloud2)
        self.sonar_pub = rospy.Publisher('sonar', Range, queue_size=5)
        rospy.Subscriber("stm_pub_ultrasonic1", Int16, self.sonarCallback)
        self.sonar_value = 0
        while not rospy.is_shutdown():
            now = rospy.Time.now()
            sonar_range = Range()
            sonar_range.header.stamp = now
            sonar_range.header.frame_id = "/sonar"
            sonar_range.radiation_type = Range.ULTRASOUND
            sonar_range.field_of_view = 0.3
            sonar_range.min_range = 0.04
            sonar_range.max_range = 0.8
            sonar_range.range = self.sonar_value/1000.0
            self.sonar_pub.publish(sonar_range)
            pcloud = PointCloud2()
            # make point cloud
            cloud = [[1,5,1],[1.1,5,1],[1.2,5,1]]
            cloud[0][1] = sonar_range.range
            cloud[1][1] = sonar_range.range
            cloud[2][1] = sonar_range.range
            pcloud.header.frame_id="/base_footprint"
            pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud)
            pub_cloud.publish(pcloud)
            rospy.loginfo(pcloud)
            rospy.sleep(1.0)

def sonarCallback(self,seq):
        self.sonar_value = seq.data

代码很简单,我就不解释了。
运行rviz,添加pointcloud2,topic主题为sonar_pcl就可以看到点云随着超声数据变化:

PCL学习之:将超声数据按照PCL点云方式发布出去的更多相关文章

  1. PCL学习八叉树

    建立空间索引在点云数据处理中有着广泛的应用,常见的空间索引一般 是自顶而下逐级划分空间的各种空间索引结构,比较有代表性的包括BSP树,KD树,KDB树,R树,四叉树,八叉树等索引结构,而这些结构中,K ...

  2. PCL学习笔记1

    先贴一段代码,从别处抄来的 #include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_type ...

  3. PCL读取PCD文件的数据

    1.pcd文件——rabbit.pcd 链接:https://pan.baidu.com/s/1v6mjPjwd7fIqUSjlIGTIGQ提取码:zspx 新建项目pcl rabbit.pcd 和p ...

  4. PCL学习(三) SAC-IA 估记object pose

    SAC-IA是基于RANSAC算法的对齐算法 通过降采样提高法向计算.FPFH特征的计算 最后通过SAC-IA计算得到对齐的旋转和平移 #include <Eigen/Core> #inc ...

  5. PCL学习(二)三维模型转点云 obj转pcd----PCL实现

    #include <pcl/io/io.h> #include <pcl/io/pcd_io.h> #include <pcl/io/obj_io.h> #incl ...

  6. ios网络学习------6 json格式数据的请求处理

    ios网络学习------6 json格式数据的请求处理 分类: IOS2014-06-30 20:33 471人阅读 评论(3) 收藏 举报 #import "MainViewContro ...

  7. Java学习-028-JSON 之二 -- 数据读取

    JSON数据由 JSONObject.JSONArray.key_value 组合而成.通常来说,JSONObject 可以包含 JSONObject.JSONArray.key_value:JSON ...

  8. Node.js学习之TCP/IP数据通讯

    Node.js学习之TCP/IP数据通讯 1.使用net模块实现基于TCP的数据通讯 提供了一个net模块,专用于实现TCP服务器与TCP客户端之间的通信 1.1创建TCP服务器 在Node.js利用 ...

  9. PCL—关键点检测(Harris)低层次点云处理

    博客转载自:http://www.cnblogs.com/ironstark/p/5064848.html 除去NARF这种和特征检测联系比较紧密的方法外,一般来说特征检测都会对曲率变化比较剧烈的点更 ...

随机推荐

  1. python写文件时,使用代码强制刷新文件

    一.实验环境 1.Windows10x64 2.anaconda4.6.9 + python3.7.1(anaconda集成,不需单独安装) 3.pyinstaller3.5 二.任务需求 三.问题描 ...

  2. Linux-Bash终端快捷键

    ^C 终正在运行的进程或放弃当前编辑的命令^U 将光标所在字符到行首之间的所有字符删除,可以使用^E到行尾再^U来删除整行内容^Z 将前台运行的进程放入背景并暂停^D 发送EOF,结束当前输入流,如果 ...

  3. mysql常用命令杂记

    查看版本 mysqladmin -uRootmaster -pRootmaster@777 versionselect version() 查看Log_bin是否开启 show variables l ...

  4. 用.net core实现反向代理中间件

    最近在将一些项目的rest api迁移到.net core中,最开始是用的Nginx做反向代理,将已经完成切换的部分切入系统,如下图所示: 由于迁移过程中也在进行代码重构,需要经常比较频繁的测试,以保 ...

  5. 关于.Net Core 部署在Linux下连接SqlServer数据库超时解决办法

    .Net Core 在 Linux 下连接 SqlServer 需要 SqlServer2008 SP3或以上版本,或SqlServer2012,或SqlServer2014. 如果SqlServer ...

  6. 获取给定地址中的域名,substring()截取

    package seday01;/** * 获取给定地址中的域名 * @author xingsir */public class Test { public static void main(Str ...

  7. 往element 模块里面渲染数据

    <template> <div class="hello"> <section class="el-container"> ...

  8. Python “ValueError: incomplete format” upon print(“stuff %” % “thingy”) 解决方法

    直接贴代码 这里我是想匹配length i  的值并且要打印出data里面%23也就是#的url编码,但是发现这样报错了,这时候我们在%23前面多加一个%号就能够成功执行我这里测试的2.7环境,3.x ...

  9. Android SearchView不显示搜索icon

    版权声明:本文为xing_star原创文章,转载请注明出处! 本文同步自http://javaexception.com/archives/80 背景: 之前碰到了一个页面展示问题,SearchVie ...

  10. Android O的通知渠道适配

     在 Android O 以后,Google引入了通知通道的概念,如果目标API大于 Android O ,不直指定通知渠道是不能发送通知的. 这里放一个我写好的通知方法,大家可以适当的改改再用,*当 ...