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:
- Get data feedback from
/odomTopic, then useeuler_from_quaternion(quaternion)function to get theyawvalue. - Create the
turnfunction that uses thatyawvalue to compute the angular speed of the robot and publish that speed to the/cmd_veltopic. Theyawvalue is constantly updated by/odomtopic and the speed of the robot will decrease gradually until the robot reach the desired degree, the robot will stop - As previously discussed, if we use any
whileloop in theturnfunction, it will block the odom callback to enter the single thread. So I create a timer, thetimer_call_back functionwill 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
super().__init__('odom_turn')
# 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(
Odometry,
'/odom',
self.odomCallback,
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
else:
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
else:
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
else:
error_angle = self.real_target_angle_2 + self.yaw
else:
# 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)
else:
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
else:
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.stop_robot()
self.destroy_timer(self.timer)
self.get_logger().info('turn completed successfully')
else:
self.real_target_angle_1_done = True
self.get_logger().info('turn partially completed')
else:
self.get_logger().info(f'target angle: {self.real_target_angle}')
self.stop_robot()
self.destroy_timer(self.timer)
self.get_logger().info('turn completed successfully')
else:
self.stop_robot()
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
self.publisher_.publish(self.vel_msg)
def timer_callback(self):
self.turn_counter_clockwise(120)
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
rclpy.init(args=args)
# declare the node constructor
simple_odom_turn = TurtlebotSimpleOdom()
# pause the program execution, waits for a request to kill the node (ctrl+c)
rclpy.spin(simple_odom_turn)
# Explicity destroy the node
simple_odom_turn.destroy_node()
# shutdown the ROS communication
rclpy.shutdown()
if __name__ == '__main__':
main()
Please help,
Thanks