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)

  • 相关阅读:
    一篇与面试官和蔼交流的深入了解JVM(JDK8)
    逆向工程,调试Hello World !程序(更新中)
    SpingBoot + Dubbo + Zookeeper实现简单分布式开发的应用
    Vue Axios 切片上传文件含实时进度
    Vue入门——常见指令及其详细代码示例
    女儿说要看烟花,但是政府规定不能放,程序员爸爸默默的拿起了键盘,程序员就是要为所欲为!
    逆向工程,调试Hello World !程序(更新中)
    python学习初始函数
    Python学习之装饰器
    Python学习之装饰器进阶
  • 原文地址:https://www.cnblogs.com/gary-guo/p/10141408.html
Copyright © 2011-2022 走看看