ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

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()
pose_pub.publish(robo_pose)
print("pose is:", robo_pose)
rate.sleep()

if __name__ == '__main__':
move()

edit retag close merge delete

Sort by ยป oldest newest most voted

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

more

Thanks a lot, it worked!

( 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?

( 2019-12-12 14:22:08 -0600 )edit