#!/usr/bin/env python
# -*- coding: utf- -*-
import rospy
import math
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Pose, Quaternion,PoseWithCovarianceStamped
import PyKDL def quat_to_angle(quat):
rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
return map(normalize_angle,rot.GetRPY()) def normalize_angle(angle):
res = angle
while res > math.pi:
res -= 2.0*math.pi
while res < -math.pi:
res += 2.0*math.pi
return res def callback(data):
#rpy
print(quat_to_angle(data.orientation))
#回调函数 收到的参数.data是通信的数据 默认通过这样的 def callback(data) 取出data.data数据 def getangle(orientation):
x=orientation.x
y=orientation.y
z=orientation.z
w=orientation.w f=*(w*y-z*z) r = math.atan2(*(w*x+y*z),-*(x*x+y*y))
p=
if(-<=f<=):
p = math.asin(f)
y = math.atan2(*(w*z+x*y),-*(z*z+y*y)) angleR = r*/math.pi
angleP = p*/math.pi
angleY = y*/math.pi return {"angleR":angleR,"angleP":angleP,"angleY":angleY} def listener(): # In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True) #启动节点同时为节点命名, 若anoymous为真则节点会自动补充名字,实际名字以 listener_322345等表示
#若为假,则系统不会补充名字,采用用户命名。但是一次只能有一个同名节点,若后面有一个相同listener
#名字的节点则后面的节点启动会注销前面的相同节点名。 rospy.Subscriber("xxx_imu_driver/imu",Imu, callback)
#rospy.Subscriber("robot_pose",Pose, callback) #启动订阅,订阅主题,及标准字符串格式,同时调用回调函数,当有数据时调用函数,取出数据 # spin() simply keeps python from exiting until this node is stopped #循环程序
rospy.spin() if __name__ == '__main__':
listener()
#函数不被用作模块调用

python ros 订阅imu数据,实时显示欧拉角的更多相关文章

  1. 使用Python控制1602液晶屏实时显示时间(附PyCharm远程调试)

    前言 原创文章,转载引用务必注明链接.水平有限,如有疏漏,欢迎指正. 本文介绍一下UP板的GPIO资源使用,以及一个使用Python演示一个简单的demo. 本文使用Markdown写成,为获得更好的 ...

  2. python ros 订阅robot_pose获取机器人位置

    #!/usr/bin/env python import rospy import tf from tf.transformations import * from std_msgs.msg impo ...

  3. ROS在rviz中实时显示轨迹(nav_msgs/Path消息的使用)

    消息结构说明nav_msgs/Path.msg结构#An array of poses that represents a Path for a robot to followHeader heade ...

  4. [转]ROS订阅激光数据

    https://github.com/robopeak/rplidar_ros/blob/master/src/client.cpp /*   * Copyright (c) 2014, RoboPe ...

  5. AngularJS checkbox/复选框 根据缓存数据实时显示

    想缓存选择按钮时,可以使用这样的方法. index.html <!DOCTYPE html> <html data-ng-app="App"> <he ...

  6. 利用动态图层实现数据的实时显示(arcEngine IDynamiclayer)

    marine 原文利用动态图层实现数据的实时显示(arcEngine IDynamiclayer) 说明:最近一个项目用到这方面知识,文章主要来至网络,后期会加入自己的开发心得.(以下的代码实例中,地 ...

  7. ROS实时采集Android的图像和IMU数据

    前言       临近毕业,整理一下之前做的东西.这篇博客来自于博主在2016年3月份投的一篇会议论文(论文主要介绍了一个基于手机摄像头和IMU的简单VIO系统,用于AR的Tracking部分,本博文 ...

  8. python实现websocket服务器,可以在web实时显示远程服务器日志

    一.开始的话 使用python简单的实现websocket服务器,可以在浏览器上实时显示远程服务器的日志信息. 之前做了一个web版的发布系统,但没实现在线看日志,每次发布版本后,都需要登录到服务器上 ...

  9. python websocket网页实时显示远程服务器日志信息

    功能:用websocket技术,在运维工具的浏览器上实时显示远程服务器上的日志信息 一般我们在运维工具部署环境的时候,需要实时展现部署过程中的信息,或者在浏览器中实时显示程序日志给开发人员看.你还在用 ...

随机推荐

  1. JQuery 的优先级

    1.使用最新的jQuery版本 2.用对选择器.   2.1 jquery最快的选择器是ID选择器:来源于js的getElementById()方法 注释:需要选择多个元素,必然涉及到Dom遍历和循环 ...

  2. mysql允许root远程登录

    MySQL 默认不允许远程以root进行登陆 进入mysql后 mysql>select host, user from user; +--------------------+------+ ...

  3. WPF不同方式快捷键判断

    private void Window_PreviewKeyDown(object sender, KeyEventArgs e) { //单个按键e.Key方式判断 if (e.Key == Key ...

  4. Suricata Rules

    Suricata Rules https://redmine.openinfosecfoundation.org/projects/suricata/wiki/Suricata_Rules https ...

  5. linux高频操作: host,用户管理,免密登陆,管道,文件权限,脚本,防火墙,查找

    1. 修改hosts和hostname 2. 用户管理 3. 免秘登陆 4. 文件末尾添加 >> 5. 设置可执行文件 6. 任何地方调用 7. Centos6 永久关闭防火墙 8. Ce ...

  6. English-培训6-Do you like rap?

  7. iOS testflight 使用说明

    一.告知开发者苹果手机的账户邮箱 1.通过任何形式告知即可 2.下载testflight 二.开发者发送激活邮件到测试者的账户邮箱 1.进入邮箱查看邮件点击 Accept invitation 进行下 ...

  8. cd .ssh返回-bash: cd: .ssh:No such file or directory怎么办

    继续上一篇博文 今天再次陷入同样的问题 避免这个问题的另一个套路是用节点和其他节点直接ssh 远程连接,需要输入密码, 但是输入再次退出之后就OK了 cd 可以到.ssh了  然后就可以开心的免密了

  9. RestFramework之权限组件

    一.权限组件的使用 1.首先需要导入包 from rest_framework.permissions import BasePermission 2.编写权限类 class VipPermissio ...

  10. Springboot整合cxf后不能访问controller,不能访问接口

    参考版本 springboot 1.4.X <=========> cxf-spring-boot-starter-jaxws 3.1.X springboot 1.5.X <=== ...