Primeros pasos con ROS - Programación de acciones

El cliente envía un objetivo en movimiento para simular el proceso del robot moviéndose a la posición objetivo, incluida la implementación del código del servidor y el cliente, lo que requiere retroalimentación de la posición en tiempo real.

¿Qué es una acción?

Un robot es un sistema inteligente complejo. No es tan simple como el movimiento del control remoto del teclado y el reconocimiento de un objetivo determinado. Lo que debemos lograr es un robot que satisfaga las necesidades de escenarios específicos, como la entrega de alimentos, la entrega y la clasificación. .

En la realización de estas funciones de aplicación, a menudo se utiliza otro mecanismo de comunicación ROS, es decir, acciones. El significado de este concepto se puede entender bien por el nombre.El propósito de este mecanismo de comunicación es facilitar la gestión del proceso de un comportamiento completo del robot.

Las acciones se pueden resumir a grandes rasgos en los siguientes cuatro puntos:

Un mecanismo de comunicación de preguntas y respuestas.
con retroalimentación continua
Se puede cancelar durante una tarea
Implementación de mecanismo de mensajes basado en ROS

modelo de comunicación

Por ejemplo, si queremos que el robot gire en círculo, definitivamente no es algo que se pueda hacer con un clic, el robot tiene que girar poco a poco hasta terminar en 360 grados, supongamos que el robot no está frente a él. nuestros ojos ¿Sabes si el robot comenzó a dar vueltas y hacia dónde fue?

OK, lo que necesitamos ahora es una retroalimentación. Por ejemplo, cada 1s, díganos a cuántos grados hemos girado, 10 grados, 20 grados, 30 grados. Después de un tiempo, cuando lleguemos a 360 grados, enviaremos otro mensaje. para indicar que la acción se ha completado

Para un comportamiento de este tipo que necesita ejecutarse durante un período de tiempo, es más adecuado utilizar el mecanismo de comunicación de la acción, al igual que instalar una barra de progreso, podemos controlar el progreso en cualquier momento, y si el movimiento está en progreso, también podemos enviar un comando para cancelar el movimiento en cualquier momento

¿Cómo definir el mensaje de acción?

Aquí creamos un nuevo paquete de funciones:

catkin_create_pkg learning_action roscpp rospy std_msgs geometry_msgs turtlesim message_generation actionlib_msgs actionlib

Cree una nueva carpeta en el directorio del paquete de funciones, asígnele el nombre acción y cree un archivo vacío TurtleMove.action en esta carpeta

Defina el contenido del mensaje de acción en el archivo Motion.action:

# Define the goal
float64 turtle_target_x  # Specify Turtle's　target position
float64 turtle_target_y
float64 turtle_target_theta
---
# Define the result
float64 turtle_final_x
float64 turtle_final_y
float64 turtle_final_theta
---
# Define a feedback message
float64 present_turtle_x
float64 present_turtle_y
float64 present_turtle_theta

La interfaz de comunicación de acción (Acción) proporciona cinco definiciones de mensaje, a saber, objetivo, cancelación, estado, retroalimentación y resultado

objetivo: objetivo de la tarea de liberación
cancelar: solicitud para cancelar la tarea
estado: Notifica al cliente del estado actual
retroalimentación: tarea de retroalimentación periódica que ejecuta datos de monitoreo
result: Envía el resultado de ejecución de la tarea al cliente, solo se publica una vez

而 .action 文件用来定义其中三种消息，按顺序分别为 goal、result 和f eedback，与 .srv 文件中的服务消息定义方式一样，使用“—”作为分隔符

这里我们直接修改功能包目录下的 package.xml 文件，添加如下内容

然后我们就需要进入到我们功能包目录下的 src 目录下，设计我们的代码文件，这里仅给出 c++ 代码：

点击查看完整代码：

TurtleMove_client.cpp

#include <actionlib/client/simple_action_client.h>
#include "learn_action/TurtleMoveAction.h"
#include <turtlesim/Pose.h> 
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>

typedef actionlib::SimpleActionClient<learn_action::TurtleMoveAction> Client;

struct Myturtle
{
    float x;
    float y;
    float theta;
}turtle_present_pose;

// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
        const learn_action::TurtleMoveResultConstPtr& result)
{
    ROS_INFO("Yay! The TurtleMove is finished!");
    ros::shutdown();
}

// 当action激活后会调用该回调函数一次
void activeCb()
{
    ROS_INFO("Goal just went active");
}

// 收到feedback后调用该回调函数
void feedbackCb(const learn_action::TurtleMoveFeedbackConstPtr& feedback)
{
    ROS_INFO(" present_pose : %f  %f  %f", feedback->present_turtle_x,
                   feedback->present_turtle_y,feedback->present_turtle_theta);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "TurtleMove_client");

    // 定义一个客户端
    Client client("TurtleMove", true);

    // 等待服务器端
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    // 创建一个action的goal
    learn_action::TurtleMoveGoal goal;
    goal.turtle_target_x = 1;
    goal.turtle_target_y = 1;
    goal.turtle_target_theta = 0;

    // 发送action的goal给服务器端，并且设置回调函数
    client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);

    ros::spin();

    return 0;
}

TurtleMove_server.cpp

/*
　　　此程序通过通过动作编程实现由client发布一个目标位置
　　　然后控制Turtle运动到目标位置的过程
 */
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "learn_action/TurtleMoveAction.h"
#include <turtlesim/Pose.h> 
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>

typedef actionlib::SimpleActionServer<learn_action::TurtleMoveAction> Server;

struct Myturtle
{
    float x;
    float y;
    float theta;
}turtle_original_pose,turtle_target_pose;

ros::Publisher turtle_vel;

void posecallback(const turtlesim::PoseConstPtr& msg) 
{
  ROS_INFO("Turtle1_position:(%f,%f,%f)",msg->x,msg->y,msg->theta);
  turtle_original_pose.x=msg->x; 
  turtle_original_pose.y=msg->y;
  turtle_original_pose.theta=msg->theta;
 }

// 收到action的goal后调用该回调函数
void execute(const learn_action::TurtleMoveGoalConstPtr& goal, Server* as)
{
    learn_action::TurtleMoveFeedback feedback;

    ROS_INFO("TurtleMove is working.");
    turtle_target_pose.x=goal->turtle_target_x;
    turtle_target_pose.y=goal->turtle_target_y; 
    turtle_target_pose.theta=goal->turtle_target_theta;

    geometry_msgs::Twist vel_msgs;
    float break_flag;

    while(1)
    {
        ros::Rate r(10);

        vel_msgs.angular.z = 4.0 * (atan2(turtle_target_pose.y-turtle_original_pose.y,
                                   turtle_target_pose.x-turtle_original_pose.x)-turtle_original_pose.theta);
        vel_msgs.linear.x = 0.5 * sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +
                                      pow(turtle_target_pose.y-turtle_original_pose.y, 2)); 
        break_flag=sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +
                                        pow(turtle_target_pose.y-turtle_original_pose.y, 2));
        turtle_vel.publish(vel_msgs);

        feedback.present_turtle_x=turtle_original_pose.x;
        feedback.present_turtle_y=turtle_original_pose.y;
        feedback.present_turtle_theta=turtle_original_pose.theta;
        as->publishFeedback(feedback);
        ROS_INFO("break_flag=%f",break_flag);
        if(break_flag<0.1) break;
        r.sleep();
    }
        // 当action完成后，向客户端返回结果
        ROS_INFO("TurtleMove is finished.");
        as->setSucceeded();
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "TurtleMove_server");
    ros::NodeHandle n,turtle_node;
    //订阅小乌龟的位置信息
    ros::Subscriber sub = turtle_node.subscribe("turtle1/pose",10,&posecallback);
    //发布控制小乌龟运动的速度
    turtle_vel = turtle_node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);    
    // 定义一个服务器
    Server server(n, "TurtleMove", boost::bind(&execute, _1, &server), false);
    // 服务器开始运行
    server.start();
    ROS_INFO("server has started.");
    ros::spin();

    return 0;
}

这里我们的客户端调用回调函数进行接收消息，只有接收到客户端启用的消息，我们才能发送运动的目标终点，在 server 中同样使用对应的回调函数，检测到 client 中发来的信息才能控制小海龟运行

下面就是我们最常见的修改 CMakeLists.txt 的部分：

修改如下部分：

添加如下内容：

add_executable(TurtleMove_client src/TurtleMove_client.cpp)
target_link_libraries(TurtleMove_client ${catkin_LIBRARIES})
add_dependencies(TurtleMove_client ${PROJECT_NAME}_gencpp)

add_executable(TurtleMove_server src/TurtleMove_server.cpp)
target_link_libraries(TurtleMove_server ${catkin_LIBRARIES})
add_dependencies(TurtleMove_server ${PROJECT_NAME}_gencpp)