ROS 2 in 5 days Python, Part 3 Topics Quiz, problem with rotate function

Hi, The Construct Team,

I am currently on the Topic quiz. I used the idea of this quiz to create a program that rotates the robot to the desired degree using two topics /odom and /cmd_vel. The algorithm is:

  1. Get data feedback from /odom Topic, then use euler_from_quaternion(quaternion) function to get the yaw value.
  2. Create the turn function that uses that yaw value to compute the angular speed of the robot and publish that speed to the /cmd_vel topic. The yaw value is constantly updated by /odom topic and the speed of the robot will decrease gradually until the robot reach the desired degree, the robot will stop
  3. As previously discussed, if we use any while loop in the turn function, it will block the odom callback to enter the single thread. So I create a timer, the timer_call_back function will call the turn function every 0.5s

It’s really worked. But my program can only rotate once, If I want the robot to rotate at a different angle, I have to stop the program, modify the code, and rebuild. Because my turn function is called in the timer, after every rotation, I have to destroy the timer, If I don’t, the robot will rotate forever. However, If I use the while loop in the turn function instead of the timer, the odom_callback will be blocked. Is there any way to rotate the robot easier? At this point, I have run out of ideas!

Here is my code:

import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
# import the Twist interface from the geometry_msgs package
from geometry_msgs.msg import Twist
# import the Odometry interface from the nav_msgs package
from nav_msgs.msg import Odometry
# import Quality of Service library, to set the correct profile and reliability to read sensor data.
from rclpy.qos import ReliabilityPolicy, QoSProfile
import numpy as np

class TurtlebotSimpleOdom(Node):
    def __init__(self):
        # Here you have the class constructor
        # call super() in the constructor to initialize the Node object
        # the parameter you pass is the node name

        # create the subscriber object
        # in this case, the subscriptor will be subscribed on /odom topic with a queue size of 10 messages.
        # use the Odometry module for /odom topic
        # send the received info to the odomCallback method.
        self.subscriber = self.create_subscription(
            QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT))

        # create the publisher object
        # in this case, the publisher will publish on /cmd_vel Topic with a queue size of 10 messages.
        # use the Twist module for /cmd_vel Topic
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)

        # define the timer period for 0.5 seconds
        self.timer_period = 0.5
        # create a timer sending two parameters:
        # - the duration between 2 callbacks (0.5 seconds)
        # - the timer function (timer_callback)
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

        # initialize the message that will be sent to cmd_vel topic to move the robot
        self.vel_msg = Twist()

        # initial orientation
        self.yaw = 0.0

        # set flag to detemine turn function is called the first time
        self.first_call = True

        self.real_target_angle = 0.0
        self.real_target_angle_1 = 999.0
        self.real_target_angle_2 = 999.0
        self.real_target_angle_1_done = False

    def turn_counter_clockwise(self,target_angle):

        if self.first_call == True:
            if self.yaw < 0:
                self.real_target_angle = target_angle*np.pi/180.0 + self.yaw
                if abs(self.real_target_angle + np.pi) > 0.02 and self.real_target_angle > 0.0:
                    self.real_target_angle_1 = self.yaw
                    self.real_target_angle_2 = target_angle*np.pi/180.0 + self.real_target_angle_1
                self.real_target_angle = target_angle*np.pi/180.0 + self.yaw
                if abs(self.real_target_angle - np.pi) > 0.02 and self.real_target_angle > 3.0:
                    self.real_target_angle_1 = np.pi - self.yaw
                    self.real_target_angle_2 = np.pi - (target_angle*np.pi/180.0 - self.real_target_angle_1)

            self.first_call = False

        if self.real_target_angle_1 != 999.0 and self.real_target_angle_1 < 0:
            if self.real_target_angle_1_done == False:
                error_angle = self.yaw
                error_angle = self.real_target_angle_2 - self.yaw

        elif self.real_target_angle_1 != 999.0 and self.real_target_angle_1 > 0:
            if self.real_target_angle_1_done == False:
                error_angle = np.pi - self.yaw
                error_angle = self.real_target_angle_2 + self.yaw
            # compare target angle with actual angle (yaw) of the robot
            error_angle = self.real_target_angle - self.yaw

        self.get_logger().info(f'error_angle: {error_angle}')
        if rclpy.ok():
            if abs(error_angle) > 0.005:
                command_angle = 0.12*abs(error_angle)
                command_vel = 0.0
                self.publish_vel(command_vel, command_angle)
                if self.real_target_angle_1 != 999.0:
                    self.get_logger().info(f'target angle 1: {self.real_target_angle_1}')
                    self.get_logger().info(f'target angle 2: {self.real_target_angle_2}')
                    if self.real_target_angle_2 > 0:
                        pi_different = self.real_target_angle_2 - np.pi
                        pi_different = self.real_target_angle_2 + np.pi
                    # self.get_logger().info(f'target angle 2: {self.real_target_angle_2}')

                    if self.real_target_angle_1_done == True or \
                        abs(self.real_target_angle_2) < 0.015 or \
                        abs(pi_different) < 0.015:
                        self.get_logger().info('turn completed successfully')
                        self.real_target_angle_1_done = True
                        self.get_logger().info('turn partially completed')
                    self.get_logger().info(f'target angle: {self.real_target_angle}')
                    self.get_logger().info('turn completed successfully')
            self.get_logger().info('robot is stopped by command line')

    def publish_vel(self,vx,wz):
        self.vel_msg.linear.x = vx
        self.vel_msg.angular.z = wz
    def timer_callback(self):

    def odomCallback(self, odom_msg):
        self.current_position = odom_msg.pose.pose
        yaw = get_angle_from_pose(self.current_position)
        self.yaw = yaw

        self.get_logger().info(f'yaw: {self.yaw}')

def euler_from_quaternion(quaternion):
    Converts quaternion (w in last place) to euler roll, pitch, yaw
    quaternion = [x, y, z, w]
    Bellow should be replaced when porting for ROS 2 Python tf_conversions is done.
    x = quaternion[0]
    y = quaternion[1]
    z = quaternion[2]
    w = quaternion[3]

    sinr_cosp = 2 * (w * x + y * z)
    cosr_cosp = 1 - 2 * (x * x + y * y)
    roll = np.arctan2(sinr_cosp, cosr_cosp)

    sinp = 2 * (w * y - z * x)
    pitch = np.arcsin(sinp)

    siny_cosp = 2 * (w * z + x * y)
    cosy_cosp = 1 - 2 * (y * y + z * z)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

def get_angle_from_pose(pose):
    orient_list = [pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w]
    (roll,pitch,yaw) = euler_from_quaternion(orient_list)

    return yaw

def main(args=None):
    # initialize the ROS communication

    # declare the node constructor
    simple_odom_turn = TurtlebotSimpleOdom()

    # pause the program execution, waits for a request to kill the node (ctrl+c)

    # Explicity destroy the node
    # shutdown the ROS communication

if __name__ == '__main__':

Please help,

Hi @ThanhVinhCE ,

Welcome to the Community,

Your odom_callback seems to be fine, but your timer_callback's turn_counter_clockwise algorithm seems to be huge (or otherwise time-complex). Keep your algorithm simple and easy as much as possible.

Taking a guess at your situation - why your robot goes into a continuous rotation mode - I believe is because your timer_callback's single routine takes more than 0.5 seconds and thus the robot is not able to converge to a state before the timer ends.
In simpler words, your timer callback gets called every 0.5 seconds, but due to the time complexity of your algorithm turn_counter_clockwise which takes more than 0.5 seconds to complete, your robot keeps skipping on the final stop point, thus making your robot rotate indefinitely.

You must either:
make sure that your algorithm completes before 0.5 seconds (or whatever callback time you specify)
simplify your algorithm to converge into a completion state faster.

To make your convergence faster, you must make your timer callback time smaller. It is a trade-off between timer callback time and algorithm time - which you should balance correctly.

I hope that was helpful.


Hi @girishkumar.kannan

Thank you for your answer.

But I think you are misunderstanding my question. Firstly, let me explain how my turn_counter_clockwise operates, for example, if I want my robot to rotate 90 degrees (target_angle = 90):

  1. This function will get the yaw value and the real_target_angle is computed by 90 + yaw. I assume that the robot’s initial yaw is at 0 degrees, so the real_target_angle = 90 degrees
  2. The error_angle = 90 - yaw, the robot’s angle velocity: command_angle = 0.12*abs(error_angle)
  3. Publish the robot’s angle velocity every 0.5s:
command_angle = 0.12*abs(error_angle)
command_vel = 0.0
self.publish_vel(command_vel, command_angle)

Because my robot has rotated, the yaw will increase every 0.5s (0, 10, 20, 30, … 90) and the error_angle ( = 90 - yaw) will decrease every 0.5s (90, 80, 70, 60, … 0). So, robot’s angle velocity will decrease gradually and it will reach 0 when it rotates to 90 degrees, the robot stops rotating.

As you can see, this function doesn’t finish the robot’s rotation in 0.5s, It updates the robot’s angle velocity every 0.5s and this velocity will decrease every 0.5s until it reaches 0.

I have tested it, and this function worked well. But it can only rotate once. Every time I want the robot to rotate at a different angle, I have to modify the angle passed into the turn_counter_clockwise function. More clearly, If my robot has rotated 60 degrees and I want it to continue to rotate 90 degrees, I have to stop my program, modify the angle from 60 to 90, rebuild, and re-run.

My question is how I can rotate my robot at as many angles as I want by just calling turn_counter_clockwise(60), turn_counter_clockwise(90), and turn_counter_clockwise(100) without modifying and rebuilding my program

I hope after what I explained, you were able to understand my question clearly.

If you have any ideas or suggestions for me, please feedback.

Thanh Vinh

Hi @ThanhVinhCE ,

I can now understand your problem better. Thanks for the clear explanation.

I believe the only way to set the angle value automatically within the program (and not set it every time and rebuild again) would be to make use of another sensing device or any other information with which you can change the required angle to turn during program runtime.

My doubt here is : how would you know what angle to turn without further information?
I did go through the Topics Quiz again and noted that you need to make the TurtleBot turn a 90 degree to pass through that gap in the wall. How would you know where to make that 90 degree turn without additional information?

Assuming you have that additional information of [where to turn] and [how much to turn], you just have to start your program with your function turn_counter_clockwise set to 0.0 initially and when you want to turn, you use a boolean flag to perform the turn until the turn is complete.

So that way, when you have to go straight, your turn is 0.0 so your robot keeps going straight and when you want to make a turn, enable the boolean flag to turn and set the turn angle. Once the turn is complete, reset the boolean flag and continue going straight by resetting the angle value to 0.0.

I hope I was helpful. Let me know if you need more clarity.


Hi @girishkumar.kannan,

I have already finished this task, thank you for your support.

I also have some additional information if you work with a real robot. In my case, I use turtlebot3 burger robot:

  1. The lidar of turtlebot3 burger only scans accurate distances within 1.7 meters. If the space is out of this range, the /scan topic will return 0.0 and the robot does not move. To solve that problem, we have to check whether the distance equals 0.0. For example, if we want the robot to go straight until the distance from the obstacle to the robot is less than 0.5 meters, we should use:
if self.laser_msg.ranges[359] > 0.5 or self.laser_msg.ranges[359] == 0.0:

Instead of:

if self.laser_msg.ranges[359] > 0.5:
  1. When rotating the robot, the angular velocity will decrease gradually until the robot reaches the angular we desired. But, when the actual rotation is so close to the one we desired, the angular speed will be very small, it’s ok with simulation but with the actual robot, that speed is not enough for the robot to rotate. So, we should set the specific angular velocity, if the rotation is so close to the one we desired. For me, I use this code:
if abs(error_angle) > 0.03:
    command_angle = 0.8*abs(error_angle)
    command_angle = 0.008
command_vel = 0.0
self.publish_vel(command_vel, command_angle)

The error_angle is the difference between the actual rotation and the rotation we desired.

That’s it. I hope my additional information was helpful.

Thanh Vinh

1 Like

This topic was automatically closed after 13 hours. New replies are no longer allowed.