Ask Your Question
0

Can anyone please help me, struggling to convert between quaternions and euler angles.

asked 2019-12-12 04:15:28 -0600

hrushi19 gravatar image

updated 2019-12-12 07:25:04 -0600

gvdhoorn gravatar image

Straight copy of the script:

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped
from math import sin
from math import cos

twist=Twist()
def callback(data):
    global twist
        twist=data
        print("value of twist cb is:",twist)
def move():
    rospy.init_node('move')
    rospy.Subscriber("cmd_vel",Twist,callback)
    pose_pub=rospy.Publisher("robo_pose",PoseStamped,queue_size=10)
    robo_pose=PoseStamped()

    while not rospy.is_shutdown():

        rate= rospy.Rate(10)
                robo_pose.pose.position.x+=((twist.linear.x*0.1)*cos(robo_pose.pose.orientation.z))+             ((twist.linear.y*0.1)*sin(robo_pose.pose.orientation.z))
        robo_pose.pose.position.y+=((twist.linear.y*0.1)*cos(robo_pose.pose.orientation.z))+((twist.linear.x*0.1)*sin(robo_pose.pose.orientation.z))
        robo_pose.pose.orientation.z+=(twist.angular.z*0.1)
        robo_pose.pose.orientation.normalize()
        robo_pose.header.stamp = rospy.Time.now()
            robo_pose.header.frame_id="map"  
        pose_pub.publish(robo_pose)
        print("pose is:", robo_pose)
        rate.sleep()

if __name__ == '__main__':
    move()
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2019-12-12 06:19:47 -0600

wintermute gravatar image

from euler to quaternion: q = quaternion_from_euler(roll, pitch, yaw)

from quarternion to euler angles: roll, pitch, yaw = euler_from_quaternion(q)

do not forget to add includes and imports.

~best

-CA

edit flag offensive delete link more

Comments

Thanks a lot, it worked!

hrushi19 gravatar imagehrushi19 ( 2019-12-12 12:33:00 -0600 )edit

if you think that the answer worked, could you click the mark this answer as correct button on the left side?

wintermute gravatar imagewintermute ( 2019-12-12 14:22:08 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-12-12 01:59:14 -0600

Seen: 28 times

Last updated: Dec 12 '19