Can anyone please help me, struggling to convert between quaternions and euler angles.
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()