Objetivo: Implementar servidor y cliente de acciones en C++.
Nivel del tutorial: Intermedio
Tiempo: 15 minutos
contenido
fondo
Las acciones son una forma de comunicación asincrónica en ROS. El cliente de acción envía una solicitud de destino al servidor de acción . El servidor de acciones envía comentarios y resultados del objetivo al cliente de acciones .
requisitos previos
Necesitará el paquete y la interfaz definidos en la acción de creación del tutorial anterior.action_tutorials_interfacesFibonacci.action
Tarea
1 Crear paquete action_tutorials_cpp
Como vimos en el tutorial Cree su primer paquete ROS 2 , necesitamos crear un nuevo paquete para contener nuestro C++ y el código de soporte.
1.1 Crear paquete action_tutorials_cpp
Vaya al espacio de trabajo de operaciones que creó en el tutorial anterior (recuerde proporcionar una fuente para el espacio de trabajo) y cree un nuevo paquete para el servidor de operaciones de C++:
cd ~/action_ws/src ros2 pkg crear --dependencias action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
1.2 Agregar control de visibilidad
Para que el paquete se compile y funcione en Windows, necesitamos agregar algunos "controles de visibilidad". Los detalles sobre por qué es necesario esto se pueden encontrar aquí .
Ábrelo action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.hy pon el siguiente código:
#ifndef ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#define ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#ifdef __cplusplus
extern "C"
{
#endif
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport))
#define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport))
#else
#define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport)
#define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport)
#endif
#ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL
#define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT
#else
#define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT
#endif
#define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC
#define ACTION_TUTORIALS_CPP_LOCAL
#else
#define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default")))
#define ACTION_TUTORIALS_CPP_IMPORT
#if __GNUC__ >= 4
#define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default")))
#define ACTION_TUTORIALS_CPP_LOCAL __attribute__ ((visibility("hidden")))
#else
#define ACTION_TUTORIALS_CPP_PUBLIC
#define ACTION_TUTORIALS_CPP_LOCAL
#endif
#define ACTION_TUTORIALS_CPP_PUBLIC_TYPE
#endif
#ifdef __cplusplus
}
#endif
#endif // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_2. Escribir un servidor de acciones
Centrémonos en escribir un servidor de acciones que calcule la secuencia de Fibonacci usando la acción que creamos en el tutorial Crear acciones.
2.1 Escribir código del servidor de acciones
Ábrelo action_tutorials_cpp/src/fibonacci_action_server.cppy pon el siguiente código:
#include <functional>
#include <memory>
#include <thread>
#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "action_tutorials_cpp/visibility_control.h"
namespace action_tutorials_cpp
{
class FibonacciActionServer : public rclcpp::Node
{
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
ACTION_TUTORIALS_CPP_PUBLIC
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)
{
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
this->get_node_base_interface(),
this->get_node_clock_interface(),
this->get_node_logging_interface(),
this->get_node_waitables_interface(),
"fibonacci",
std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
std::bind(&FibonacciActionServer::handle_cancel, this, _1),
std::bind(&FibonacciActionServer::handle_accepted, this, _1));
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
using namespace std::placeholders;
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
}
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
rclcpp::Rate loop_rate(1);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<Fibonacci::Feedback>();
auto & sequence = feedback->partial_sequence;
sequence.push_back(0);
sequence.push_back(1);
auto result = std::make_shared<Fibonacci::Result>();
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
// Check if there is a cancel request
if (goal_handle->is_canceling()) {
result->sequence = sequence;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
// Update sequence
sequence.push_back(sequence[i] + sequence[i - 1]);
// Publish feedback
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "Publish feedback");
loop_rate.sleep();
}
// Check if goal is done
if (rclcpp::ok()) {
result->sequence = sequence;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
}; // class FibonacciActionServer
} // namespace action_tutorials_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)
Las primeras líneas contienen todos los archivos de encabezado que necesitamos compilar.
A continuación creamos una clase que es una clase derivada de rclcpp::Node:
clase FibonacciActionServer: público rclcpp::Nodo
El constructor de esta clase FibonacciActionServerinicializa el nombre del nodo en fibonacci_action_server:
explícito FibonacciActionServer(const rclcpp::NodeOptions & opciones = rclcpp::NodeOptions())
: Nodo("fibonacci_action_server", opciones)
El constructor también crea una instancia de un nuevo servidor de acciones:
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
this->get_node_base_interface(),
this->get_node_clock_interface(),
this->get_node_logging_interface(),
this->get_node_waitables_interface(),
"fibonacci",
std::bind( &FibonacciActionServer::handle_goal, esto, _1, _2),
std::bind(&FibonacciActionServer::handle_cancel, esto, _1),
std::bind(&FibonacciActionServer::handle_accepted, esto, _1));
Un servidor de acción requiere 6 cosas:
-
Nombre del tipo de operación con plantilla:
Fibonacci. -
Nodo ROS 2 al que agregar la operación:
this. -
Nombre de la acción:
'fibonacci'. -
Función de devolución de llamada para procesar el objetivo:
handle_goal -
Función de devolución de llamada para gestionar la cancelación:
handle_cancel. -
La función de devolución de llamada utilizada para manejar el destino aceptar
handle_accept:.
Lo siguiente en el archivo es la implementación de varias devoluciones de llamada. Tenga en cuenta que todas las devoluciones de llamada deben regresar rápidamente; de lo contrario, podríamos bloquear al ejecutor.
Comenzamos manejando la devolución de llamada para el nuevo objetivo:
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> objetivo)
{
RCLCPP_INFO(this->get_logger(), "Solicitud de objetivo recibida con orden %d", objetivo-> orden);
(nulo)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
Esta implementación solo acepta todos los objetivos.
La siguiente es la devolución de llamada que maneja la cancelación:
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Se recibió solicitud para cancelar objetivo");
(nulo)goal_handle;
return rclcpp_action::CancelResponse::ACEPTAR;
}
Esta implementación simplemente le dice al cliente que aceptó la cancelación.
La última devolución de llamada acepta un nuevo objetivo y comienza a procesarlo:
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
usando el espacio de nombres std::placeholders;
// esto debe regresar rápidamente para evitar bloquear al ejecutor, así que inicie un nuevo hilo
std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
}
Dado que la ejecución es una operación de larga duración, generamos un hilo para realizar el trabajo real y handle_acceptedregresar rápidamente.
executeTodo el procesamiento y las actualizaciones adicionales se realizan en los métodos del nuevo hilo:
void ejecutar(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Ejecutando objetivo");
rclcpp::Tasa loop_rate(1);
const auto objetivo = goal_handle->get_goal();
retroalimentación automática = std::make_shared<Fibonacci::Retroalimentación>();
auto & secuencia = retroalimentación->partial_sequence;
secuencia.push_back(0);
secuencia.push_back(1);
resultado automático = std::make_shared<Fibonacci::Resultado>();
for (int i = 1; (i < objetivo->orden) && rclcpp::ok(); ++i) { //
Comprobar si hay una solicitud de cancelación
if (goal_handle->is_canceling()) {
resultado-> secuencia = secuencia;
goal_handle->cancelado(resultado);
RCLCPP_INFO(this->get_logger(), "Objetivo cancelado");
devolver;
}
// Actualizar secuencia
secuencia.push_back(secuencia[i] + secuencia[i - 1]);
// Publicar comentarios
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "Publicar comentarios");
loop_rate.sleep();
}
// Comprobar si el objetivo se cumplió
if (rclcpp::ok()) {
resultado->sequence = secuencia;
goal_handle->éxito(resultado);
RCLCPP_INFO(this->get_logger(), "Objetivo logrado");
}
}
Este hilo de trabajo procesa un número de secuencia de Fibonacci por segundo y publica una actualización de comentarios para cada paso. Cuando termina de procesarse, marca goal_handleel éxito y sale.
Ahora tenemos un servidor de acciones completamente funcional. Vamos a construirlo y ejecutarlo.
2.2 Servidor de acciones de compilación
En la sección anterior, implementamos el código del servidor de acciones. Para que se pueda compilar y ejecutar, necesitamos hacer algunas cosas adicionales.
Primero necesitamos configurar CMakeLists.txt para poder compilar el servidor de acciones. Abra action_tutorials_cpp/CMakeLists.txty agregue lo siguiente después de las llamadas find_package:
add_library(action_server SHARED
src/fibonacci_action_server.cpp)
target_include_directories(action_server PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(action_server
PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_server
"action_tutorials_interfaces"
"rclcpp"
" rclcpp_action"
"rclcpp_components")
rclcpp_components_register_node(action_server PLUGIN "action_tutorials_cpp::FibonacciActionServer" EJECUTABLE fibonacci_action_server)
instalar(OBJETIVOS
action_server
ARCHIVO DESTINO lib
BIBLIOTECA DESTINO lib
contenedor de DESTINO DE TIEMPO DE EJECUCIÓN)
Ahora podemos compilar el paquete. Vaya al nivel superior de action_wsy ejecute:
construcción de colon
Esto debería compilar todo el espacio de trabajo, incluidos fibonacci_action_serverlos paquetes action_tutorials_cpp.
2.3 Ejecutar el servidor de acciones
Ahora que hemos creado el servidor de acciones, podemos ejecutarlo. Obtenga el espacio de trabajo que acabamos de crear ( action_ws) e intente ejecutar el servidor de acciones:
ros2 ejecuta action_tutorials_cpp fibonacci_action_server
3. Cliente de acción de escritura
3.1 Escribir código de cliente de acción
Ábrelo action_tutorials_cpp/src/fibonacci_action_client.cppy pon el siguiente código:
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
namespace action_tutorials_cpp
{
class FibonacciActionClient : public rclcpp::Node
{
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
{
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this->get_node_base_interface(),
this->get_node_graph_interface(),
this->get_node_logging_interface(),
this->get_node_waitables_interface(),
"fibonacci");
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));
}
void send_goal()
{
using namespace std::placeholders;
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
rclcpp::shutdown();
}
auto goal_msg = Fibonacci::Goal();
goal_msg.order = 10;
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionClient::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
{
auto goal_handle = future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
void feedback_callback(
GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
std::stringstream ss;
ss << "Next number in sequence received: ";
for (auto number : feedback->partial_sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}
void result_callback(const GoalHandleFibonacci::WrappedResult & result)
{
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
std::stringstream ss;
ss << "Result received: ";
for (auto number : result.result->sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
rclcpp::shutdown();
}
}; // class FibonacciActionClient
} // namespace action_tutorials_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient)Las primeras líneas contienen todos los archivos de encabezado que necesitamos compilar.
A continuación creamos una clase que es una clase derivada de rclcpp::Node:
clase FibonacciActionClient: público rclcpp::Nodo
El constructor de esta clase FibonacciActionClientinicializa el nombre del nodo en fibonacci_action_client:
explícito FibonacciActionClient(const rclcpp::NodeOptions & options)
: Nodo("fibonacci_action_client", opciones)
El constructor también crea una instancia de un nuevo cliente de acción:
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this->get_node_base_interface(),
this->get_node_graph_interface(),
this->get_node_logging_interface(),
this->get_node_waitables_interface(),
"fibonacci");
Un cliente de acción requiere tres cosas:
-
Nombre del tipo de operación con plantilla:
Fibonacci. -
Agregue el cliente de acción al nodo ROS 2:
this. -
Nombre de la acción:
'fibonacci'.
También creamos una instancia de un temporizador ROS que iniciará una única llamada send_goal:
this->timer_ = this->create_wall_timer(
std::chrono::millisegundos(500),
std::bind(&FibonacciActionClient::send_goal, this));
Cuando el temporizador expira, llama send_goal:
void send_goal()
{
usando el espacio de nombres std::placeholders;
esto->temporizador_->cancelar();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "El servidor de acciones no está disponible después de esperar");
rclcpp::apagar();
}
auto goal_msg = Fibonacci::Goal();
goal_msg.order = 10;
RCLCPP_INFO(this->get_logger(), "Enviando objetivo");
auto send_goal_options = rclcpp_action::Cliente<Fibonacci>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionClient::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
Esta función hace lo siguiente:
-
Cancela el cronómetro (por lo que solo se llama una vez).
-
Espere a que aparezca el servidor de acciones.
-
Crear una instancia nueva
Fibonacci::Goal. -
Establezca devoluciones de llamadas de respuesta, comentarios y resultados.
-
Envía el objetivo al servidor.
Cuando el servidor recibe y acepta el objetivo, envía una respuesta al cliente. Esta respuesta es manejada por goal_response_callback:
void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> futuro)
{
auto goal_handle = futuro.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "El servidor rechazó el objetivo");
} else {
RCLCPP_INFO(this->get_logger(), "Objetivo aceptado por el servidor, esperando resultado");
}
}
Suponiendo que el servidor haya aceptado el objetivo, comenzará a procesarse. Cualquier comentario a los clientes será manejado por feedback_callback:
void feedback_callback(
GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> comentarios)
{
std::stringstream ss;
ss << "Siguiente número en secuencia recibido: ";
for (número automático: retroalimentación->secuencia_partial) {
ss << número << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}
Una vez que el servidor complete el procesamiento, devolverá un resultado al cliente. Los resultados son procesados por result_callback:
void result_callback(const GoalHandleFibonacci::WrappedResult & result)
{
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "El objetivo fue abortado");
devolver;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "El objetivo fue cancelado");
devolver;
predeterminado:
RCLCPP_ERROR(this->get_logger(), "Código de resultado desconocido");
devolver;
}
std::stringstreamss;
ss << "Resultado recibido: ";
for (número automático: resultado.resultado->secuencia) {
ss << número << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
rclcpp::apagar();
}
Ahora tenemos un cliente de acción completamente funcional. Vamos a construirlo y ejecutarlo.
3.2 Compilar acción cliente
En la sección anterior, implementamos el código del cliente de acción. Para que se pueda compilar y ejecutar, necesitamos hacer algunas cosas adicionales.
Primero necesitamos configurar CMakeLists.txt para poder compilar el cliente de acciones. Abra action_tutorials_cpp/CMakeLists.txty agregue lo siguiente después de las llamadas find_package:
add_library(action_client SHARED
src/fibonacci_action_client.cpp)
target_include_directories(action_client
PRIVADO $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(action_client
PRIVADO "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_client
"action_tutorials_interfaces"
"rclcpp"
" rclcpp_action"
"rclcpp_components")
rclcpp_components_register_node(action_client PLUGIN "action_tutorials_cpp::FibonacciActionClient" EJECUTABLE fibonacci_action_client)
install(OBJETIVOS
action_client
ARCHIVO DESTINO lib
BIBLIOTECA DESTINO lib
contenedor de DESTINO DE TIEMPO DE EJECUCIÓN)
Ahora podemos compilar el paquete. Vaya al nivel superior de action_wsy ejecute:
construcción de colon
Esto debería compilar todo el espacio de trabajo, incluidos fibonacci_action_clientlos paquetes action_tutorials_cpp.
3.3 Ejecutar cliente de acción
Ahora que hemos creado el cliente de acciones, podemos ejecutarlo. Primero asegúrese de que el servidor de acciones se esté ejecutando en una terminal separada. Ahora obtenga el espacio de trabajo que acabamos de crear ( action_ws) e intente ejecutar el cliente de acciones:
ros2 ejecuta action_tutorials_cpp fibonacci_action_client
Debería ver un mensaje de registro indicando que se aceptó el objetivo, que se están imprimiendo los comentarios y el resultado final.
generalizar
En este tutorial, armó un servidor de acciones C++ y un cliente de acciones línea por línea y los configuró para intercambiar objetivos, comentarios y resultados.