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:
ROS (Robot Operating System) se está convirtiendo en el “framework” estándar para programar robots. En este tutorial emplearemos los conceptos de servicios para comandar el robot “Turtlebot3” haciendo uso de una interfaz de servicio personalizada que crearemos.Todo esto es simulado en un mundo de Gazebo. Ten en cuenta que se usará ROS2 Humble para este tutorial.
PASO 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.
Primero crearemos el paquete con las dependencias necesarias. Tener en cuenta que, para crear una interfaz de servicio personalizada solo puede hacer con un paquete CMake. Para ello ejecutamos en un terminal lo siguiente:
Luego crearemos dentro del paquete la carpeta srv que contendrá la interfaz de servicio. Dentro de este creamos el archivo Mover.srv
cd ~/ros2_ws/src/tutorial_interface_srv
mkdir srv
cd srv
touch Mover.srv
Dentro de este archivo colocamos el contenido de la interfaz que necesitamos, el cual se compone de la parte de petición (request) y respuesta (response), que se separan mediante 3 guiones (—). Toda esta estructura permite la comunicación entre cliente y servidor.
Dentro de el archivo recién creado colocar lo siguiente: Mover.srv
float64 angulo # Angulo a girar
float64 velocidad_lineal # Velocidad Lineal (en m/s)
int32 tiempo # Duración del movimiento (en segundos)
---
bool completado # Se logró?
PASO 4:Modificando CMakeLists.txt y package.xml
Una vez creado el archivo del servicio personalizado, necesitamos modificar los siguientes archivos para que nuestro paquete compile correctamente:
– CMakeLists.txt: Para convertir las interfaces definidas en código específico de un lenguaje (como C++ y Python) para que puedan utilizarse en esos lenguajes.
– package.xml: Añadimos las dependencias adicionales necesarias.
Con lo cual CMakeList.txt deberia quedarnos mínimante con lo siguiente:
cmake_minimum_required(VERSION 3.8)
project(tutorial_interface_srv)
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(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/Mover.srv"
)
ament_package()
Por último, compilamos el paquete para poder usar la interfaz de servicio creada.
cd ~/ros2_ws
colcon build --packages-select tutorial_interface_srv
source install/setup.bash
Es importante realizar el último paso, de lo contrario no se encontrará la interfaz de servicio que se generó. Para asegurarnos de que interfaz se generó correctamente usamos los siguiente:
ros2 interface show tutorial_interface_srv/srv/Mover
Esto nos debería mostrar el contenido de Mover.srv en la terminal:
PASO 6: Usando la interfaz
Para hacer uso de la interfaz, se creo un paquete (tutorial_servicios) que contiene un servidor y un cliente que se comunican mediante el mensaje Mover.srv que acabamos de generar.
El cliente hace una petición al servidor enviándole el ángulo que necesita girar, la velocidad lineal y el tiempo que debe moverse con esa velocidad. Una vez realizado el movimiento el servidor enviará un mensaje de confirmación al cliente como respuesta. Para ello, ejecutamos la simulación en Gazebo (la cual se puede obtener desde este link).
En este tutorial, aprenderás el proceso de portar un paquete de ROS 1, creado originalmente en SolidWorks, a un formato URDF compatible con ROS 2. Exploraremos los pasos esenciales para migrar tu diseño robótico al nuevo entorno, asegurando la compatibilidad y funcionalidad en la última versión de ROS. Sigue la guía detallada para una transición sin complicaciones y mantén tus proyectos robóticos al día con las últimas tecnologías.
Abriendo el rosject
Para poder seguir al pie de la letra este tutorial, necesitamos tener ROS 2 Humble instalado en nuestro sistema, e idealmente un workspace para crear los paquetes en él. Para facilitarnos el proceso, ya hemos preparado un rosject para esto: https://app.theconstructsim.com/l/5e643001/
Solo copiando el rosject (Abriendo el enlace anterior), tendremos un entorno previamente configurado.
Después de que el rosject se haya copiado correctamente en tu entorno, debemos presionar el botón Ejecuta para ejecutar e inicializar el entorno del rosject.
[Inserta imagen del Rosject]
Crear un paquete de ROS 2 con ament_python
Para poder crear nuestro paquete de ROS 2, necesitamos situarnos en el workspace, y para eso, necesitamos la terminal.
Abramos una terminal oprimiendo el botón de abrir una nueva terminal:
[Inserte imagen de la terminal]
Una vez dentro de la terminal vamos a movernos al ros2_ws/src y creamos nuestro paquete: cd ~/ros2_ws/src
ros2 pkg create lidar3d --build-type ament_python --dependencies rclpy
Una vez creado nuestro paquete ingresamos a él y copiamos el contenido de nuestro paquete generado por Solidworks: cd lidar3d
cp -r ../lidar3d/* .
Ahora vamos a abrir el editor de código para modificar nuestros archivos. Para abrir el editor de código oprimimos el botón de editor de código:
[Inserte imagen de vscode]
Nos situamos en ros2_ws/src/lidar3d/urdf y abrimos el archivo de urdf:
[Inserte imagen del URDF]
Primero vamos a añadir un nuevo objeto vacío de base_link con un joint al base_link generado por Solidworks ya que rviz 2 no soporta base_link con inercia y le cambiamos el nombre al base_link de Solidworks:
[Inserte imagen de vscode con urdf modificado]
Ahora debido a que modificamos el nombre del paquete, vamos a usar la función de sustituir palabra integrada con Vscode:
[inserte imagen de sustitución]
El siguiente paso es ir a ros2_ws/src/lidar3d/launch para crear un launch de Python compatible con ROS 2, para eso damos click derecho y Nuevo archivo y le ponemos de nombre display.launch.py
[inserte imagen del launch file]
Por ultimo antes de compilar, debemos decirle a nuestro paquete donde se encuentran los solidos URDF los meshes y los launch files, para eso nos vamos a ros2_ws/src/lidar3d y añadimos las respectivas instrucciones install para que encuentre nuestras carpetas:
[Inserte imagen del package modificado]
Ya que tenemos nuestro paquete configurado podemos compilar y ejecutar nuestro launch file para poder generar las configuraciones por defecto de rviz2
Para poder activar las configuraciones requeridas de rviz 2 vamos a modificar nuesto Fixed Frame y cambiamos map por base_link, ahora le damos a añadir y para poder depurar mejor nuestro objeto añadimos el arbol de TF con la opcion TF, y por último vamos a añadir el modelo de nuestro robot añadiendo RobotModel:
[Inserte imagen de rviz]
Como podemos observar, aun no se ve nuestro robot, para eso debemos en la sub-opción de Description Topic seleccionar el topico de robot_description
ROS (Robot Operating System) se está convirtiendo en el “framework” estándar para programar robots. En este tutorial emplearemos los conceptos de tópicos y nodos para mover el robot “Turtlebot3” hacia múltiples posiciones deseadas. Hacemos uso de los mensajes del tópico /odom para conocer la posición estimada del robot y publicar en el tópico /cmd_vel para mover el robot a las posiciones deseadas. Todo esto es simulado en un mundo de Gazebo. Ten encuesta que se usará ROS2 Humble para este tutorial.
PASO 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.theconstructsim.com/l/5fc977f5/.
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.
El Turtlebot3 se moverá por una serie de posiciones en un orden específico, es decir, se moverá de su posición inicial a la 1ra posición deseada, una vez alcanzada, se moverá a la 2da posición y así sucesivamente. Para lograr esto, necesitamos de una estrategia.
Tengamos en cuenta que el robot conocerá las posiciones que debe alcanzar y su posición actual estimada gracias al tópico /odom, del cual aprovecharemos la posición en X, posición Y, y la rotación en Z.
Con ello, para alcanzar cada posición el Turtlebot3 realizará 2 movimientos: Primero, girar hasta que quede mirando a la posición deseada; Segundo, avanzar de manera recta hasta llegar a la posición.
Adicionalmente, debemos tener en cuenta el robot no se moverá en una línea recta de manera perfecta, por lo que se requiere compensar esa desviación conforme avanza. Esto se puede realizar añadiendo una velocidad angular cuando la orientación cambie demasiado.
Creación del paquete
Primero creamos un paquete en python para realizar un nodo-publicador-subscriptor que mueva al Turtlebot3 de la forma deseada. Necesitamos añadir las dependencias necesarias. Para ello ejecutamos en un terminal lo siguiente:
Debido a que en el tópico /odom la orientación es expresada en cuaterniones, el método euler_from_quaternion es necesario para convertir la orientación del robot de cuaterniones a la notación roll-pitch-yaw. Esto nos permite obtener la rotación del robot alrededor del eje z (yaw).
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
Este es el método definido en el timer y contiene la estrategia de control explicada anteriormente. Se ejecuta cada 0.1s y actualiza la velocidad del robot de acuerdo con la posición estimada.
Finalmente simularemos el comportamiento en gazebo, para ello primero iniciamos la simulación del Turtlebot3, cuyo paquete ya esta en este rosject. Esta se puede obtener desde este link.
ROS (Robot Operating System) se está convirtiendo en el “framework” estándar para programar robots. En esta publicación, aprenderemos a usar adecuadamente Robot State Publisher para publicar las transformaciones TF de los sistemas coordenados de nuestro robot. Así como, lo necesario para visualizar el robot en RViz y Gazebo.
Abriendo el rosject
Para seguir este tutorial, necesitamos tener instalado ROS en nuestro sistema, y lo ideal sería tener un catkin_ws (Espacio de Trabajo ROS). Para facilitarte la vida, ya hemos preparado un rosject para eso: https://app.theconstructsim.com/l/5e2843be/.
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.
Iniciando Robot State Publisher y visualizando el robot en RViz
Para usar Robot State Publisher necesitamos de la descripción de un robot en archivos urdf o xacro. En este rosject usaremos archivos xacro de un robot móvil simple, los cuales están en la carpeta robot de nuestro paquete robot_tutorial.
Robot a usar en el tutorial – mobile_robot
Creamos un archivo launch robot_visualization.launch para iniciar robot_state_publisher. Para ello, en un nuevo terminal colocamos lo siguiente: cd /home/user/catkin_ws/src/robot_tutorial
mkdir launch
touch launch/robot_visualization.launch
En ese archivo creado colocamos:
robot_visualization.launch <launch>
<!-- Cargar el URDF en el servidor de parámetros de ROS -->
<param name="robot_description" command="$(find xacro)/xacro $(find robot_tutorial)/robot/mobile_robot.urdf.xacro" />
<!-- Iniciamos robot_state_publisher para publicar las tranformaciones tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
output="screen">
<param name="publish_frequency" type="double" value="5.0" />
</node>
</launch>
Con esto:
Cargamos la descripción del robot en el parámetro robot_description.
Iniciamos el nodo robot_state_publisher que tomará la descripción del robot para publicar las transformaciones con una frecuencia de 5Hz.
Previamente, antes de iniciar el launch nos aseguramos de compilar nuestro paquete, para ello, en un terminal colocamos:
cd /home/user/catkin_ws
catkin_make
source devel/setup.bash
rospack profile
Con ello, ya podemos iniciar nuestro archivo launch: roslaunch robot_tutorial robot_visualization.launch
Al iniciar el launch y visualizar el árbol TF usando rosrun rqt_tf_tree rqt_tf_tree obtenemos que no están presentes las transformaciones de las juntas no fijas, es decir, de las ruedas.
Árbol de tf publicado por robot_state_publisher – incompleto
Esto se debe a que robot_state_publisher necesita que se publiquen los valores de las juntas no fijas para hacer las transformaciones, de lo contrario solo publicará las trasformaciones estáticas (juntas fijas).
Por ello debemos añadir un nodo que publique los valores de las juntas de alguna manera. Usaremos para ello joint_state_publisher_gui. Añadimos lo siguiente al archivo recién creado:
Iniciamos el nodo joint_state_publisher_gui para que publique los valores de las juntas de las ruedas.
Iniciamos RViz con una configuración que guardaremos en launch/robot.rviz
Con ello ya vemos las transformaciones faltantes al usar rqt_tf_tree y también podemos ver el robot en RViz.
Árbol de tf publicado por robot_state_publisher – completo
Robot en RViz – completo
cd /home/user/catkin_ws/src/robot_tutorial
mkdir config
touch config/robot_control.yaml
En este archivo nuevo, colocaremos el controlador joint_state_controller, que será el encargado de publicar los valores de las juntas no fijas del robot simulado en Gazebo.
robot_control.yaml
# Publicar todos los estados de las juntas (/joint_states) -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
Posteriormente, creamos otro archivo launch donde colocaremos lo necesario para la simulación. cd /home/user/catkin_ws/src/robot_tutorial
touch launch/spawn_robot.launch
En este archivo, colocamos lo siguiente: <launch>
<arg name="x" default="0.0" />
<arg name="y" default="0.0" />
<arg name="z" default="0.2" />
<arg name="robot_name" default="mobile_robot" />
<!-- Cargar el URDF en el servidor de parámetros de ROS -->
<param name="robot_description" command="$(find xacro)/xacro $(find robot_tutorial)/robot/mobile_robot.urdf.xacro" />
Establecer una posición de aparición del robot en el mundo de Gazebo.
Asignamos un nombre al robot (mobile_robot).
Cargar la descripción del robot en el parámetro robot_description.
Cargamos el controlador joint_state_controller que definimos previamente.
Iniciar el nodo robot_state_publisher con una frecuencia de publicación de 5Hz.
Iniciamos un mundo vacío en Gazebo.
Iniciamos el controlador usando el nodo controller_spawner.
Hacemos aparecer el robot en el mundo de Gazebo usando el nodo urdf_spawner pasándole los parámetros de posición, nombre y descripción del robot que previamente definimos.
Iniciamos RViz con la configuración que ya guardamos en launch/robot.rviz
Iniciamos este launch con: roslaunch robot_tutorial spawn_robot.launch
Al iniciar el launch y visualizar el árbol TF usando rosrun rqt_tf_tree rqt_tf_tree observamos el mismo problema anterior, esto se debe a que, para gazebo, es necesario colocar las transmisiones a todas las juntas no fijas al final del archivo xacro de nuestro robot.
Árbol de tf publicado por robot_state_publisher – incompleto
Robot en RViz – incompleto
Para ello nos dirigimos al archivo xacro principal de nuestro robot para añadir en la parte final lo siguiente:
Añadimos la transmisión a la junta left_wheel_joint (debe ser el mismo nombre de la junta usada en la definición del joint).
Añadimos la transmisión a la junta right_wheel_joint (debe ser el mismo nombre de la junta usada en la definición del joint).
Con ello ya se visualizan las transformaciones de todas las juntas del robot y ya tenemos el robot tanto en RVIZ como en Gazebo listo para añadir los controladores y sensores que se requieran.
Turtlebot 3 es uno de los robots compatibles con ros mas usados hoy en día en universidades y por personas del común para aprender el funcionamiento de ROS y el stack de Navegación. En este post, vamos a aprender como crear un nodo de ROS1, el cual nos va a permitir crear un teleop casero para entender como crear un suscriptor y publicador en el mismo código.
Abriendo el rosject
Para poder seguir al pie de la letra este tutorial, necesitamos tener ROS 1 Noetic instalado en nuestro sistema, e idealmente un workspace para crear los paquetes en él. Para facilitarnos el proceso, ya hemos preparado un rosject para esto: https://app.theconstructsim.com/l/5e643001/
Solo copiando el rosject (Abriendo el enlace anterior), tendremos un entorno previamente configurado.
Después de que el rosject se haya copiado correctamente en tu entorno, debemos presionar el botón Ejecuta para ejecutar e inicializar el entorno del rosject.
Crear un paquete de ROS 1 con geometry_msgs
Para poder crear nuestro paquete de ROS 1, necesitamos situarnos en el workspace, y para eso, necesitamos la terminal.
Abramos una terminal oprimiendo el botón de abrir una nueva terminal:
Una vez dentro de la terminal vamos a movernos al catkin/src y creamos nuestro paquete: cd ~/catkin_ws/src
catkin_create_pkg turtlebot_publisher_subscriber geometry_msgs
Una vez creado nuestro paquete ingresamos a él y por convención creamos la carpeta scripts: cd turtlebot_publisher_suscriber
mkdir scripts
Ahora vamos a abrir el editor de código para crear nuestro nodo. Para abrir el editor de código oprimimos el botón de editor de código:
Nos situamos en catkin_es/src/turtlebot_publisher_susbriber/scripts y oprimimos clic derecho New File y le damos el nombre de turtlebot.py para crear nuestro código
Primero vamos a importar las librerías, inicializar nuestro nodo, y crear el listener con su respectivo callback:
Para entenderlo mejor vamos a desglosar el código:
El primer rectángulo nos permite observar que estamos usando el entorno de Python 3, e importamos la librería de rospy para crear los nodos y geometry_msgs.msg Twist que es el tipo de dato Twist ya que este es el que usa /cmd_vel para comunicarse.
En el segundo rectángulo estamos creando un mensaje de tipo Twist, creamos la función listener la cual se suscribe de manera anónima a al tópico cmd_vel que usa como tipo de dato Twist, y por último cuando recibe un mensaje hace un llamado al callback que usando un loginfo imprime en pantalla el contenido del mensaje, este mensaje es la velocidad angular y lineal del turtlebot 3.
Por último en el tercer rectángulo inicializamos el nodo llamado publisher_suscriber e informamos por terminal que el nodo se inicializó correctamente.
Ahora vamos a crear el nodo publicador:
Como podemos observar creamos la función talker que recibe un mensaje como parámetro, ahora nos suscribimos al tópico /cmd_vel y publicamos el mensaje
Ahora vamos a crear una función que use el teclado para mover al turtlebot:
Lo que hace la función keyInput, esta nos pide ingresar una tecla, si se oprime w cambiamos el valor lineal de x para mover el robot hacia adelante, si se oprime s cambia el valor lineal a negativo para moverse hacia atrás, si se oprime a cambia el valor angular de z para girar hacia la izquierda y d gira hacia la derecha.
Por último, vamos a hacer llamados continuos a estas funciones añadiendo un ciclo while al código inicial:
Acá lo que hacemos es crear un ciclo while para que esté constantemente solicitando una tecla, imprimiendo la velocidad del turtlebot 3, y cambiando la velocidad del turtlebot.
Para probar el código vamos a movernos a nuestro workspace, y compilamos el entorno: cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash
Y si no tuvimos ningún error podemos correr el código y revisar si funciona
