Robotics StackExchange | Archived questions

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

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

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