zoukankan      html  css  js  c++  java
  • 四元素以及欧拉角

    c++

    头文件

    #include "tf/LinearMath/Matrix3x3.h"
    #include "geometry_msgs/Quaternion.h"
    #include "tf/transform_datatypes.h"

    欧拉角到四元素

    sensor_msgs::Imu imu_msg

    tf::Matrix3x3 rotation_matrix;
            rotation_matrix.setEulerYPR( nodes[i].gAngle * 3.1415926 / 180.0 * -1.0, 0., 0. );
            tf::Quaternion quat;
            rotation_matrix.getRotation( quat );
            imu_msg.orientation.x = quat.getX();
            imu_msg.orientation.y = quat.getY();
            imu_msg.orientation.z = quat.getZ();
            imu_msg.orientation.w = quat.getW();
    四元素到欧拉角

    geometry_msgs::Quaternion orientation = imu_msg.orientation;
            tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));
            double yaw, pitch, roll;
            mat.getEulerYPR(yaw, pitch, roll);
            ROS_INFO("The combined_odom yaw is %f ",yaw * 180 / 3.1415926);
            /*tf::Quaternion quat1;
            tf::quaternionMsgToTF(imu_msg.orientation, quat1);
            double roll, pitch, yaw;
            tf::Matrix3x3(quat1).getRPY(roll, pitch, yaw);
            ROS_INFO_STREAM("The  yaw is " << yaw * 180.0 / 3.1415926 << " " << roll << " " <<  pitch);

    python

    from tf.transformations import euler_from_quaternion

    from geometry_msgs.msg import Quaternion, Twist, Pose, QuaternionStamped
    from nav_msgs.msg import Odometry

    quaternion = Quaternion()

    quaternion.x = 0.0
                quaternion.y = 0.0
                quaternion.z = sin(self.th / 2.0)
                quaternion.w = cos(self.th / 2.0)


    odom = Odometry()
                odom.header.frame_id = "odom"
                odom.child_frame_id = self.base_frame
                #odom.child_frame_id = "baselink"
                odom.header.stamp = now
                odom.pose.pose.position.x = self.x
                odom.pose.pose.position.y = self.y
                odom.pose.pose.position.z = 0
                odom.pose.pose.orientation = quaternion
                odom.twist.twist.linear.x = vxy
                odom.twist.twist.linear.y = 0
                odom.twist.twist.angular.z = vth

     quaternion_1 = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w]
                (roll, pitch, yaw) = euler_from_quaternion(quaternion_1)
                rospy.loginfo("The wheel odom is " + str(yaw * 180 / 3.1415926))

    self.odomPub.publish(odom)

  • 相关阅读:
    判断数据类型
    css----单行文本超出部分显示省略号
    IE和火狐的事件机制有什么区别
    token的验证过程
    节点
    innerHTML, innerText, outerHTML, outerText的区别
    什么是ES5?js中的'use strict'是什么?目的是什么?
    数据库连接问题
    4.EasyUI学习总结(四)——EasyUI组件使用 (通过用户登录来演示dialog、ajax的使用,serialize方法的使用,前后台怎样交互等)
    3.EasyUI学习总结(三)——easyloader源码分析
  • 原文地址:https://www.cnblogs.com/gary-guo/p/10141408.html
Copyright © 2011-2022 走看看