rotate a desired degree using feedback from odometry
can i send a desired angle as a parameter to the programme and when I run the script it automatically rotates a desired degree using feedback from odometry
i have odom topic and my script give me an angle which the robot have to rotate .so i want to control the degree of rotation using angular velocity from Odometry this is possible
Asked by abdelkrim on 2018-05-06 06:51:29 UTC
Answers
Here the code that does what you are requesting:
#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Twist
import math
roll = pitch = yaw = 0.0
target_angle = 9
kP = 0.5
def get_rotation (msg):
global roll, pitch, yaw
orientation_q = msg.pose.pose.orientation
orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
(roll, pitch, yaw) = euler_from_quaternion (orientation_list)
print yaw
rospy.init_node('my_quaternion_to_euler')
sub = rospy.Subscriber ('/odom', Odometry, get_rotation)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
command= Twist()
r = rospy.Rate(10)
while not rospy.is_shutdown():
target_rad = target_angle * math.pi/180
command.angular.z = kP * (target_rad - yaw)
pub.publish(command)
r.sleep()
Set the desired angle in the variable target_angle
Also, if you need deeper explanation of the code check this video which describes the code step by step.
Asked by R. Tellez on 2019-10-10 18:39:48 UTC
Comments
Could you please update your answer with an explanation of your code?
Asked by jayess on 2019-10-12 13:46:48 UTC
Comments
This question is too vague, what is the program you are referring to? What is the hardware setup of your robot. We need to know these things to be able to help you.
Asked by PeteBlackerThe3rd on 2018-05-06 14:29:15 UTC
thank you for reply i modify my question to become more clear
Asked by abdelkrim on 2018-05-06 16:29:04 UTC