python ros 四元数转欧拉角
#! /usr/bin/python import PyKDL
import rospy
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from math import *
import threading
import os
import subprocess
import yaml def quat_to_angle(quat):
rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
return rot.GetRPY()[]
def normalize_angle(angle):
res = angle
while res > pi:
res -= 2.0*pi
while res < -pi:
res += 2.0*pi
return res class CalibrateRobot:
def __init__(self):
self.lock = threading.Lock()
self.sub_imu = rospy.Subscriber('imu', Imu, self.imu_cb)
self.last_imu_angle =
self.imu_angle =
while not rospy.is_shutdown():
rospy.sleep(0.3)
rospy.loginfo("imu_angle:"+str((self.imu_angle)*/3.1415926)) def imu_cb(self, msg):
with self.lock:
angle = quat_to_angle(msg.orientation)
self.imu_angle = angle
self.imu_time = msg.header.stamp def main():
rospy.init_node('scan_to_angle')
CalibrateRobot() if __name__ == '__main__':
main()
python ros 四元数转欧拉角的更多相关文章
- 3D数学基础:四元数与欧拉角之间的转换
在3D图形学中,最常用的旋转表示方法便是四元数和欧拉角,比起矩阵来具有节省存储空间和方便插值的优点.本文主要归纳了两种表达方式的转换,计算公式采用3D笛卡尔坐标系: 单位四元数可视化为三维矢量加上第四 ...
- 四元数与欧拉角(RPY角)的相互转换
RPY角与Z-Y-X欧拉角 描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式.第一种是绕固定(参考)坐标轴旋转:假设开始两个坐标系重合,先将{B}绕{A}的X轴旋转$\gamma$,然后绕{A} ...
- ROS 进阶学习笔记(13) - Combine Subscriber and Publisher in Python, ROS
Combine Subscriber and Publisher in Python, ROS This article will describe an example of Combining S ...
- eigen 中四元数、欧拉角、旋转矩阵、旋转向量
一.旋转向量 1.0 初始化旋转向量:旋转角为alpha,旋转轴为(x,y,z) Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z)) 1. ...
- ros python 四元数 转 欧拉角
import sysimport math w = -0.99114048481x = -0.00530699081719y = 0.00178255140781z = -0.133612662554 ...
- python ros 订阅imu数据,实时显示欧拉角
#!/usr/bin/env python # -*- coding: utf- -*- import rospy import math from sensor_msgs.msg import Im ...
- python 使用PyKDL 四元数转欧拉角
安装: sudo apt-get install ros-indigo-kdl-parser-py 使用: import PyKDLimport math def quat_to_angle(quat ...
- 3D数学基础(四)四元数和欧拉角
一.四元数 四元数本质上是个高阶复数,可视为复数的扩展,表达式为y=a+bi+cj+dk.在说矩阵旋转的时候提到了它,当然四元数在Unity里面主要作用也在于此.在Unity编辑器中的Transfor ...
- python ros topic demo
发布者: #!/usr/bin/env python #coding=utf- import rospy from std_msgs.msg import String def talker(): ...
随机推荐
- 雷达无线电系列(三)经典CFAR算法门限因子alpha计算(matlab)
前言 本文汇集CA.SO.GO.OS.杂波图等恒虚警算法的门限因子求解方法及其函数 1,CA-CFAR [非常简单,可以直接求解] %% 均值恒虚警_门限因子计算公式 %% 版本:v1 %% 时间:2 ...
- Python遗传和进化算法框架(一)Geatpy快速入门
https://blog.csdn.net/qq_33353186/article/details/82014986 Geatpy是一个高性能的Python遗传算法库以及开放式进化算法框架,由华南理工 ...
- net技术方案
https://www.cnblogs.com/qyq0323/p/11765243.html https://yq.aliyun.com/articles/300527
- Python爬虫之BeautifulSoap的用法
1. Beautiful Soup的简介 简单来说,Beautiful Soup是python的一个库,最主要的功能是从网页抓取数据.官方解释如下: Beautiful Soup提供一些简单的.pyt ...
- 解决github提示安全漏洞的问题
今天在提交代码的时候发现github提示了这样的错误: We found potential security vulnerabilities in your dependencies. Only t ...
- ubuntu下使用JNI Java调用C++的例子
TestJNI.java public class TestJNI { static{ System.load("/home/buyizhiyou/workspace/JNI/src/lib ...
- laravel管理模型插入
post控制器public function comment(Post $post,Request $request){ try{ if(empty($request->content)){ E ...
- lucene初探
http://www.cnblogs.com/xing901022/p/3933675.html
- Python函数Day3
一.函数名的应用 函数名类似于特殊的变量,打印函数名就是打印函数的内存地址 ① 函数名就是函数的内存地址 def func(): pass >>>func <function ...
- java相关网址汇总1
Java网站汇总 官方 框架 数据库 资源网站 视频学习网站 开发工具 其他工具 github/gitee框架项目 社区 博客/个人 官方 Sun公司网站Sun公司中文网站J2SE下载网站JavaSE ...