Manejando Threads Para Crear un Action Server – Spanish ROS Tutorial
This tutorial is created by Robotics Ambassador 017 Jose
Rosbotics Ambassador Program (https://www.theconstruct.ai/robotics-ambassador/)
Lo que vamos a aprender
- Usar múltiples threads en un nodo.
- Crear callbacks groups para el manejo de callbacks.
- Emplear un action server para comandar el Turtlebot3.
Lista de recursos usados en esta publicación
- Usa este rosject: https://app.theconstruct.ai/l/615821d8/
- The Construct: https://app.theconstructsim.com/
- Cursos ROS: ROS2 Basics in 5 Days (Python): https://app.theconstructsim.com/courses/132
- Documentación ROS Callback Groups: https://docs.ros.org/en/foxy/How-To-Guides/Using-callback-groups.html
- Documentación ROS Executors: https://docs.ros.org/en/foxy/Concepts/About-Executors.html
Resumen
ROS (Robot Operating System) se está convirtiendo en el “framework” estándar para programar robots. En este tutorial se hará uso de trheads y grupos de callbacks para poder crear un Action Server para el TurtleBot3.Todo esto es simulado en un mundo de Gazebo. Ten en cuenta que se usará ROS2 Humble para este tutorial.
1. Abriendo el rosject
Para seguir este tutorial, necesitamos tener instalado ROS2 HUMBLE en nuestro sistema, y lo ideal sería tener un ros2_ws (Espacio de Trabajo ROS2). Para facilitarte la vida, ya hemos preparado un rosject para eso: https://app.theconstruct.ai/l/603ea5b0/.
Simplemente copiando el rosject (haciendo clic en el enlace de arriba), tendrás una configuración ya preparada para ti.
Después de haber copiado el rosject a tu propia área de trabajo, deberías ver el botón RUN. Haz clic en ese botón para lanzar el rosject (abajo tienes un ejemplo de rosject).
Tras pulsar el botón RUN, deberías tener cargado el rosject. Ahora, pasemos a la siguiente sección para ponernos manos a la obra.
2. Descripción
En ROS2, los subscribers, timers, service servers y action servers usan callbacks para su funcionamiento. Al recibir un mensaje o un llamado, ejecutan algún método predefinido. Por otro lado, al crear un nodo y definir subscribers, timers, etc. de manera básica, se tiene un thread principal que se encarga de manejar los callbacks. Dicho nodo al tener solo un thread, no puede ejecutar más de un callback al mismo tiempo.
Este comportamiento hace necesario el uso de múltiples threads y callback groups para tareas más complejas que requieran que se ejecute más de un callback al mismo tiempo.
En este tutorial aplicaremos múltiples threads y callbacks para un action server que realizará lo siguiente:
- Subscribirse al tópico /odom para conocer la posición y orientación del TurtleBot3
- Calcular la distancia recorrida por el robot durante su movimiento usando un Timer
- Mover el robot por posiciones específicas que se mandarán al llamar al action server
3. Desarrollo
Múltiples threads
En esta situación es necesario el uso de más de un thread, de lo contrario, al momento de que se haga una petición al action server, se ejecutará su correspondiente callback. Esto evitará que los callbacks del subscriptor y del timer se ejecuten hasta que se termine la ejecución de su callback.
Para usar multiples threads, primero necesitamos importar MultiThreadedExecutor:
from rclpy.executors import MultiThreadedExecutorfloat64Luego definir el número de threads necesarios (2 para este caso):
# Use MultiThreadedExecutor
executor = MultiThreadedExecutor(num_threads=2)
executor.add_node(my_action_server)
try:
executor.spin()
finally:
my_action_server.destroy_node()
rclpy.shutdown()
Callback groups
Solamente definir múltiples threads no es suficiente ya que existen los callback groups.
Cuando se ejecuta un nodo en un Multi-Threaded Executor, ROS 2 ofrece dos tipos diferentes de grupos de callbacks para controlar la ejecución de estos:
- Mutually Exclusive Callback Group: Los callbacks de este grupo no se pueden ejecutar en paralelo.
- Reentrant Callback Group: Los callbacks de este grupo si pueden ejecutarse al mismo tiempo.
Se debe tener en cuenta que, los callbacks pertenecientes a diferentes grupos de callbacks (de cualquier tipo) siempre pueden ejecutarse en paralelo entre sí.
En este tutorial usaremos 2 Mutually Exclusive Callback Groups. A uno de ellos le asignaremos el callback del action server y al otro le asignaremos los callbacks del subscriptor y del timer. Esto nos permitirá que al enviarse un request al action server, los callbacks del subscriptor y del timer se sigan ejecuanto al estar en otro grupo. Adicionalmente, estos dos callbacks al estar en un mismo Mutually Exclusive Callback Group no se ejecutarán al mismo tiempo, lo cual se require ya que el subscriptor actualiza la posición actual mientras que en el timer se calcula la distancia recorrida en ese periodo de tiempo.
Para usar callback groups primero importamos el grupo a usar:
from rclpy.callback_groups import MutuallyExclusiveCallbackGroupLuego definimos los grupos:
self.mutuallyexclusive_group_1 = MutuallyExclusiveCallbackGroup()
self.mutuallyexclusive_group_2 = MutuallyExclusiveCallbackGroup()Estos grupos se tienen que asignar como argumento al momento de instanciar el subscriptor, timer y action server. Si no se realiza esto, de manera predefinida los callbacks se asignarán a un mismo Mutually Exclusive Callback Group, el cual no permite la ejecución de los callbacks en paralelo a pesar de usar múltiples threads. Entonces asignamos:
Para el subscriptor:
# Subscriber
self.subscription = self.create_subscription(
Odometry,
'odom',
self.listener_callback,
10,
callback_group=self.mutuallyexclusive_group_1)Luego definimos los grupos:
# Timer
timer_period = 0.5 # segundos
self.timer = self.create_timer(
timer_period,
self.timer_callback,
callback_group=self.mutuallyexclusive_group_1)Luego definimos los grupos:
# Action
self._action_server = ActionServer(
self, Move, 'move_as',
self.execute_callback,
callback_group=self.mutuallyexclusive_group_2)
4. Explicación del código
El action server usá una interfaz personalizada que se compone de:
- Request: 2 arrays de coordenadas para las posiciones que el robot debe alcanzar.
- Result: Distancia total recorrida durante el movimiento y un flag de confirmación de que todo se ejecuto correctamente.
- Feedback: Distancia recorrida hasta el momento y el número de la posición alcanzada.
Move.action
float32[] pos_x
float32[] pos_y
---
bool status
float32 total_dist
---
int32 nro_pos
float32 current_distEl código completo para el action server es el siguiente:
server_node.py
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from custom_interfaces.action import Move
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32
import time
import math
import numpy as np
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
class MyActionServer(Node):
def __init__(self):
super().__init__('my_action_server')
self.mutuallyexclusive_group_1 = MutuallyExclusiveCallbackGroup()
self.mutuallyexclusive_group_2 = MutuallyExclusiveCallbackGroup()
# Subscriber
self.subscription = self.create_subscription(
Odometry,
'odom',
self.listener_callback,
10,
callback_group=self.mutuallyexclusive_group_1)
# Action
self._action_server = ActionServer(
self, Move, 'move_as',
self.execute_callback,
callback_group=self.mutuallyexclusive_group_2)
# Publisher for cmd_vel
self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
def timer_callback(self):
delta_x = (self.pos[0] - self.pos_before[0])**2
delta_y = (self.pos[1] - self.pos_before[1])**2
self.distance.data += math.sqrt(delta_x + delta_y)
self.pos_before = self.pos
def listener_callback(self, msg):
self.pos = [msg.pose.pose.position.x, msg.pose.pose.position.y]
quaternion = [msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w]
self.roll, self.pitch, self.yaw = self.euler_from_quaternion(quaternion)
def euler_from_quaternion(self, quaternion):
"""
Converts quaternion (w in last place) to euler roll, pitch, yaw
quaternion = [x, y, z, w]
Below should be replaced when porting for ROS2 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 execute_movement(self, a, b):
self.vel = Twist()
# Velocidad de desplazamiento
self.vel_linear_x = 0.1
self.vel_angular_z = 0.2
# Máximo error
self.linear_error = 0.08
self.angular_error = 0.05
# Calcular ángulo de rotación
angulo_d = math.atan2(b - self.pos[1], a - self.pos[0])
while ( abs(self.yaw - angulo_d) > self.angular_error ):
self.vel.linear.x = 0.0
self.vel.angular.z = self.vel_angular_z if self.yaw < angulo_d else -self.vel_angular_z self.publisher_.publish(self.vel) time.sleep(0.5) while ( math.sqrt((self.pos[0] - a)**2 + (self.pos[1] - b)**2) > self.linear_error ):
self.vel.linear.x = self.vel_linear_x
self.vel.angular.z = 0.0
self.publisher_.publish(self.vel)
time.sleep(0.5)
self.vel.linear.x = 0.0
self.vel.angular.z = 0.0
self.publisher_.publish(self.vel)
def execute_callback(self, goal_handle):
self.get_logger().info('Executing goal...')
# Timer
timer_period = 0.5 # segundos
self.timer = self.create_timer(
timer_period,
self.timer_callback,
callback_group=self.mutuallyexclusive_group_1)
self.distance = Float32()
self.distance.data = 0.0
self.pos_before = self.pos
feedback_msg = Move.Feedback()
pos_x = goal_handle.request.pos_x
pos_y = goal_handle.request.pos_y
for i,val in enumerate(pos_x):
self.execute_movement(pos_x[i], pos_y[i])
feedback_msg.nro_pos = i + 1
feedback_msg.current_dist = self.distance.data
self.get_logger().info('Distancia recorrida hasta ahora: {0} '.format(feedback_msg.current_dist))
self.get_logger().info('Posición {0} alcanzada'.format(feedback_msg.nro_pos))
goal_handle.publish_feedback(feedback_msg)
goal_handle.succeed()
result = Move.Result()
result.total_dist = self.distance.data
result.status = True
self.get_logger().info('Distancia total recorrida: {0}'.format(result.total_dist))
return result
def main(args=None):
rclpy.init(args=args)
my_action_server = MyActionServer()
# Use MultiThreadedExecutor
executor = MultiThreadedExecutor(num_threads=2)
executor.add_node(my_action_server)
try:
executor.spin()
finally:
my_action_server.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()Donde el callback del subscriber self.listener_callback actualiza la posición y rotación del TurtleBot3, mientras que el callback del timer self.timer_callback calcula la distancia recorrida durante un periodo de 0.5 segundos.
En el método self.execute_movement es donde se ejecuta el movimiento del robot, primero rotandolo para que apunte a la posición deseada y luego desplazándolo linealmente hacia esa posición.
En el callback del action server self.execute_callback se inicia el timer, se llama al método self.execute_movement por cada posición deseada, y se envía el feedback y resultado al action cliente.
5. Simulación
Se tiene precargada una simulación del Turtlebot3 sobre la cual trabajaremos.
Para ello primero iniciamos el action server con el siguiente comando:
ros2 launch threads_tutorial server_node_launch.launch.py
Y en otra terminal hacemos el request (especificando las posiciones que el robot debe alcanzar) con el siguiente comando:
ros2 action send_goal -f /move_as custom_interfaces/action/Move "{pos_x: [2.089,1.312], pos_y: [-0.065,-0.016]}"
Con lo cual obtenemos el movimiento deseado del TurtleBot3!
