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 MultiThreadedExecutorfloat64
Luego definir el número de threads necesarios (2 para este caso):
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 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:
El 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:
Let’s assume you wish to wash your clothing. There are two possible ways you could go about it:
Go to the Laundry service provider
Put your clothes to wash.
Wait until the clothes are washed.
Get your clothes.
If you have a washing machine at home:
Put your clothes to wash
Instead of waiting, you can do other things and leave the watching machine doing its jobs
Check once in a while if the clothes are finished
Do other things.
Clothes are washed.
Option 1 is a blocking activity because you have to wait (in theory not able to do anything else) for the clothes to be washed, while option 2 is non-blocking because you can do some other things while your clothes are being washed.
This non-blocking is what defines an Action. If ROS2 Services are for instant request-responses, an Action is a task that may take a lot of time to be finished, and in the meantime, a robot (or you) is free to do other things and is also able to constantly check the status of the action.
Opening the rosject
In order to learn how to create an and use an Action Server in ROS2, we need to have ROS2 installed in our system, and it is also useful to have some simulations. To make your life easier, we already prepared a rosject with a simulation for that: https://app.theconstructsim.com/#/l/4a1c58c5/.
You can download the rosject on your own computer if you want to work locally, but just by copying the rosject (clicking the link), you will have a setup already prepared for you.
After the rosject has been successfully copied to your own area, you should see a Run button. Just click that button to launch the rosject (below you have a rosject example).
Learn ROS2 – Run rosject (example of the RUN button)
After pressing the Run button, you should have the rosject loaded. Let’s now head to the next section to really get some real practice.
Launching the simulation
The rosject we provided contains the packages needed to run a TurtleBot3 simulation in ROS2. The TurtleBot3 has the following sensors:
Lidar
IMU
Feel free to use this rosject to test your mobile robot programs.
The rosject is structured the following way:
turtlebot3_ws: this workspace contains the TurtleBot3 packages provided by ROBOTIS. Don’t modify this unless you know what you are doing and want to change something from the simulation
Use this workspace to develop your programs
Assuming you have opened the rosject by clicking the Run button, we can launch the simulation with:
After a few seconds, the simulation should have opened automatically:
[ROS2 How-to] #2 – Create a ROS2 action server – Simulation running
In case the simulation does not pop up automatically, you can easily click the Open Gazebo button like in the example below (bear in mind that the simulation below is not the one used in this tutorial. Its main purpose is to show the Open Gazebo button):
Open Gazebo by clicking Open Gazebo
Creating our ROS2 package (later used to create our Action Server)
Let’s create our ROS2 Package. For that, let’s start by opening a new terminal:
Open a new Terminal
In the terminal that was just open, by running the “ls” command you can see that we have at least the following folders:
ros2_ws turtlebot3_ws
The turtlebot3_ws contains the simulation, and the ros2_ws is where we are going to place our code.
Before you continue, it is worth mentioning that in the rosject that we shared with you, the custom_interfaces package that we are going to create here already exists. We are going to create it here basically for learning purposes. You would actually not need it since the package was already created for you:
Let’s create a package named custom_interfaces with the action_msgs std_msgs rosids_default_generators packages as dependencies:
After the package was created, let’s create a folder called action:
mkdir -p custom_interfaces/action/
and also create the action/Patrol.action file.
touch custom_interfaces/action/Patrol.action
This is the file/Interface that we will use in our Action Server for patrolling.
Let’s now open that Patrol.action file. You can open it in the Code Editor. If you do not know how to open the Code Editor, please check the image below:
Open the IDE – Code Editor
You can now open the custom_interfaces/action/Patrol.action file and paste the following content on it:
In the end, the final CMakeLiss.txt file would look like the following:
cmake_minimum_required(VERSION 3.8)
project(custom_interfaces)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(action_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
set(action_files
"action/Patrol.action"
)
rosidl_generate_interfaces(${PROJECT_NAME}
${action_files}
DEPENDENCIES action_msgs std_msgs
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
And for the file custom_interfaces/package.xml we also have to add the following code before the <export> tag:
So far so good. ROS is able to find our custom interface.
The time has now come for us to create the Action Server.
Creating our ROS2 Action Server
Let’s create a different package for the Action Server, just to keep things separated. Since we are not doing to create Interfaces in this new package, just use existing interfaces, let’s use the ament_python build type. Again, bear in mind that if you are using the rosject that we provided, the package already exists in the ~/ros2_ws/src folder:
Let’s now open that file using the Code Editor, and paste the following content to it:
#!/usr/bin/env python3
#
# Copyright 2019 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Authors: Ryan Shim, Gilbert
import math
import time
import rclpy
from geometry_msgs.msg import Twist
from rclpy.action import ActionServer
from rclpy.action import CancelResponse
from rclpy.action import GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.duration import Duration
from rclpy.node import Node
from rclpy.qos import QoSProfile
from rclpy.executors import MultiThreadedExecutor
from custom_interfaces.action import Patrol
class Turtlebot3PatrolServer(Node):
def __init__(self):
super().__init__('turtlebot3_patrol_server')
self.goal = Patrol.Goal()
qos = QoSProfile(depth=10)
# Initialise publishers
self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', qos)
# Initialise servers
self._action_server = ActionServer(
self,
Patrol,
'patrol',
execute_callback=self.execute_callback,
callback_group=ReentrantCallbackGroup(),
goal_callback=self.goal_callback,
cancel_callback=self.cancel_callback)
self.get_logger().info("Turtlebot3 patrol action server has been initialised.")
def destroy(self):
self._action_server.destroy()
super().destroy_node()
def goal_callback(self, goal_request):
# Accepts or rejects a client request to begin an action
self.get_logger().info('Received goal request :)')
self.goal = goal_request
return GoalResponse.ACCEPT
def cancel_callback(self, goal_handle):
# Accepts or rejects a client request to cancel an action
self.get_logger().info('Received cancel request :(')
return CancelResponse.ACCEPT
async def execute_callback(self, goal_handle):
self.get_logger().info('Executing goal...')
radius = self.goal.radius # unit: m
speed = 0.5 # unit: m/s
feedback_msg = Patrol.Feedback()
total_driving_time = 2 * math.pi * radius / speed
feedback_msg.time_left = total_driving_time
last_time = self.get_clock().now()
# Start executing an action
while (feedback_msg.time_left > 0):
if goal_handle.is_cancel_requested:
goal_handle.canceled()
self.get_logger().info('Goal canceled')
return Patrol.Result()
curr_time = self.get_clock().now()
duration = Duration()
duration = (curr_time - last_time).nanoseconds / 1e9 # unit: s
feedback_msg.time_left = total_driving_time - duration
self.get_logger().info('Time left until the robot stops: {0}'.format(feedback_msg.time_left))
goal_handle.publish_feedback(feedback_msg)
# Give vel_cmd to Turtlebot3
twist = Twist()
twist = self.drive_circle(radius, speed)
self.cmd_vel_pub.publish(twist)
# Process rate
time.sleep(0.010) # unit: s
# When the action is completed
twist = Twist()
self.cmd_vel_pub.publish(twist)
goal_handle.succeed()
result = Patrol.Result()
result.success = True
self.get_logger().info('Returning result: {0}'.format(result.success))
return result
def drive_circle(self, radius, velocity):
self.twist = Twist()
self.linear_velocity = velocity # unit: m/s
self.angular_velocity = self.linear_velocity / radius # unit: rad/s
self.twist.linear.x = self.linear_velocity
self.twist.angular.z = self.angular_velocity
return self.twist
def main(args=None):
rclpy.init(args=args)
patrol_action_server = Turtlebot3PatrolServer()
# Use a MultiThreadedExecutor to enable processing goals concurrently
executor = MultiThreadedExecutor()
rclpy.spin(patrol_action_server, executor=executor)
patrol_action_server.destroy()
rclpy.shutdown()
if __name__ == '__main__':
main()
The code used above is just an adaptation of a code already provided by ROBOTIS.
Before we compile our code, we also have to open the patrol_action_server/setup.py file and modify the entry_points section to define our executable called patrol_action_server_exe in the following way:
We can now run our server with the following commands:
source install/setup.bash
ros2 run patrol_action_server patrol_action_server_exe
The server should start with no problems:
[INFO] [1651528559.914166370] [turtlebot3_patrol_server]: Turtlebot3 patrol action server has been initialised
Calling our ROS2 Action Server
Ok, if you did not kill the Action Server launched in the previous section, please open a second terminal that we will use to call the Action Server.
With “ros2 node list” we should be able to find our node running:
ros2 node list
/turtlebot3_patrol_server
And with ros2 action list, we should be able to see the /patrol action:
ros2 action list
/patrol
We can now call our Action Server. If you remember when we created the Patrol.action, we defined a radius. Let’s them call the Action Server passing a radius of 0.5. The robot will be basically rotating:
You should now see the feedback sent by the action server:
Waiting for an action server to become available...
Sending goal:
radius: 0.5
Goal accepted with ID: dd32bc835d7a4ef5ae854d0bfb4b119f
Feedback:
time_left: 6.2831525802612305
Feedback:
time_left: 6.271763801574707
Feedback:
time_left: 6.260392665863037
Feedback:
time_left: 6.2484917640686035
Feedback:
time_left: 6.237414836883545
Feedback:
time_left: 6.2265496253967285
Feedback:
time_left: 6.215761661529541
...
^CCanceling goal...
Feedback:
time_left: 5.634908676147461
Goal canceled.
Remember that you can easily cancel the call to the action server by pressing CTRL+C.
If you look at the simulation after sending a goal to the Action Server, you should see the robot spinning around 0.5 meters.
Congratulations. You now know how to create a ROS2 Action Server from scratch. If you want more details about the code of the Action Server, please check the video in the next section.
Youtube video
So this is the post for today. Remember that we have the live version of this post on YouTube. If you liked the content, please consider subscribing to our youtube channel. We are publishing new content ~every day.
Keep pushing your ROS Learning.
Related Courses & Training
If you want to learn more about ROS and ROS2, we recommend the following courses:
![[ROS2 How-to] #2 – Create a ROS2 action server](https://www.theconstruct.ai/wp-content/uploads/2022/05/how-to-use-action-server-in-ros2.jpeg)
![[ROS2 How-to] #2 - Create a ROS2 action server - Simulation running](https://www.theconstruct.ai/wp-content/uploads/2022/05/ROS2-How-to-2-Create-a-ROS2-action-server-Simulation-running.png)
