In this video we are going to see how to rotate our robot based on data from odometry. We will subscribe to /odom topic to get the heading of our robot, process it, and then send a command over the /cmd_vel topic.

This is a video based on the following post on ROS Answers: https://answers.ros.org/question/290534/rotate-a-desired-degree-using-feedback-from-odometry

// RELATED LINKS

▸ Original question: https://goo.gl/GSHi42
▸ ROS Development Studio (ROSDS): https://goo.gl/bUFuAq
▸ Robot Ignite Academy: https://goo.gl/6nNwhs

[irp posts=”7999″ name=”ROS Q&A – How to convert quaternions to Euler angles”]

Step 1. Create a project in ROS Development Studio(ROSDS)

We can do any ROS development we want easily, without setting up environment locally in ROSDS and that’s the reason why we will use ROSDS for this tutorial. If you haven’t had an account yet. You can create one here for free now. After logging in, let’s begin our journey by clicking on the create new project and call it rotate_robot.

Step 2. Create package

At first, we launch the simulation from Simulations->Turtlebot 2

We’ll create a package for the rotating robot task with the following command

cd ~/catkin_ws/src
catkin_create_pkg rotate_robot rospy

Then we’ll create a script folder and a main.py script inside it with the following content

#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion, quaternion_from_euler

roll = pitch = yaw = 0.0

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)

r = rospy.Rate(1)
while not rospy.is_shutdown():
    quat = quaternion_from_euler (roll, pitch,yaw)
    print quat
    r.sleep()

Then we can run the script with the following command to see the topic publishing in quaternion and Euler angle.

cd rotate_robot/script
chmod +x main.py
rosrun rotate_robot main.py

You can also run the teleop script to move the robot and see the value changing with this command

rosrun  teleop_twist_keyboard teleop_twist_keyboard.py

Step 3. Rotate the robot

We’ll use the easiest way to do it by applying the controller. Please save the script with a new name rotate.py and change it as the following content.

#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from geometry_msgs.msg import Twist
import math

roll = pitch = yaw = 0.0
target = 90
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('rotate_robot')

sub = rospy.Subscriber ('/odom', Odometry, get_rotation)
pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
r = rospy.Rate(10)
command =Twist()

while not rospy.is_shutdown():
    #quat = quaternion_from_euler (roll, pitch,yaw)
    #print quat
    target_rad = target*math.pi/180
    command.angular.z = kp * (target_rad-yaw)
    pub.publish(command)
    print("taeget={} current:{}", target,yaw)
    r.sleep()

After executing it, you should see the robot turn to the desired orientation.

 

Want to learn more?

Applying the controller is not the only way to do it. There are more advanced and better ways to do it. If you are interested, please check our ROS Basic and TF ROS 101 courses  in  Robot Ignite Academy for more information.

 

 

Edit by: Tony Huang

 

---

 

Feedback

Did you like this video? Do you have questions about what is explained? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know on the comments area and we will do a video about it.