Action
导言
我们在前面已经学习了两种非常强力的通讯方式：Topic，Service，但我们发现这两个通讯方式想要在一个完整的动作过程中给出反馈是比较困难的——事实上前面所说的阻塞线程的问题也是可以解决的，但是要保持恰好的频率进行反馈信息的发送，这也就意味着难度比较大
另一方面，代码写起来也非常难受，因为要使用一个 msg 和一个 srv 的接口，这也就意味着代码会比较冗长
在我们的闭环控制系统中，ROS2 本身就给出了一种通讯方式来完成实时反馈的动作，也就是我们今天要讲的 Action
浅谈Action
Action 的主要功能可以用下面的一个过程来描述：
- 控制器向执行器发出开始动作的请求以及动作的目标，执行器返回开始执行的信息
- 执行器正在执行，持续向控制器发出反馈信号，中途如果收到控制器的停止信号，则停下
- 当执行到目标后，停止并返回给控制器
上面这个描述很明显比较适合 FOP 的编程思维，那么我们接下来就转化成 OOP 的描述吧：
Action的三个主要组成部分：
- 目标：客户端向服务端传输目标信号，服务端针对目标给出响应
- 反馈：执行过程中服务端向客户端实时汇报进度如何
- 结果：服务端执行到结束时向客户端返回执行结果，表示最终状态
这里我们来根据图解释一下 Action 内在的节点通讯流程：
这里我们会发现一个 Action 接口包实际上像是包含了两个 Service 和一个 Topic 的接口包，事实上也就是如此，因为反馈是单向的，而目标和结果都是双向通讯的，这样的封装是相当合理的，也就能完整描述一个控制动作的过程
自定义通信接口
在前面两种通讯方式的学习中，尤其在 Service 的学习过程中，我们发现市面上其实并没有太多已经封装好的消息接口能够完成我们需要完成的任务，所以事实上，我们自己在编写通讯的时候，往往都需要自定义通讯接口
那接下来我们完整展示单独创建一个功能包的过程
当然再此之前，我们要分出我们的学习区域：
cd ros2_learning
mkdir -p Action/action_cpp_ws/src
创建功能包：
cd Action/action_cpp_ws/src
ros2 pkg create robot_control_interfaces --build-type ament_cmake
这里我们的接口功能包暂时是不需要引用其他接口的，所以这里就不需要添加其他接口包的依赖，如果有依赖，则需要手动添加包，也就是使用--dependencies xxx的指令，下面我们会进行一次演示（这里不添加--dependencies rosidl_default_generators，下面在功能包中手动添加展示，加深印象）
接下来在我们的功能包创建接口文件：
mkdir -p robot_control_interfaces/action
cd robot_control_interfaces/action
touch MoveRobot.action
接下来我们要修改一下配置文件：
packages.xml
<depend>rosidl_default_generators</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
这里的第一行就是因为没有添加--dependencies rosidl_default_generators所以要补充的部分
CmakeList.txt
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/MoveRobot.action"
)
这里的第二行也是我们没有写--dependencies rosidl_default_generators所以需要添加的
然后就是我们具体的接口文件：
MoveRobot.action
float32 distance
---
float32 pose
---
float32 pose
uint32 status
uint32 STATUS_MOVING = 1
uint32 STATUS_STOP = 2
上面的内容中，我们同样根据分割线分成三部分：目标、结果、反馈
这样就可以根据三个部分需要传输的数据内容进行自定义了
这样我们的文件就编写完成了
接下来就可以进行编译生成接口
cd /action_cpp_ws
colcon build --packages-select robot_control_interfaces
在完成创建后，我们应该可以在/install中看到 C++ 和 Python 的消息文件目录：
C++
install/robot_control_interfaces/include
Python
install/robot_control_interfaces/local/lib/python3.10
在此基础上，我们完成了对于 Action 接口的创建，下面我们就可以开始创建我们的机器人节点和控制节点了
C++
编写机器人及控制节点
功能包创建
首先还是一样，在同一个工作空间下创建我们的节点功能包：
cd action_cpp_ws/src
ros2 pkg create robor_demo_rclcpp --build-type ament_cmake --dependencies rclcpp rclcpp_action robot_control_interfaces
cd robot_demo_rclcpp/src/
touch action_control_01.cpp
touch action_robot_01.cpp
这里我们需要使用我们 C++ 多文件编写的方法，相信看这篇文章的都是有 C++ 基础的，就不多说了
继续创建Robot类：
touch robot.cpp
cd ..
touch include/robot_demo_rclcpp/robot.h
展示一下创建完成后的功能包结构：
.
├── CMakeLists.txt
├── include
│ └── robot_demo_rclcpp
│ └── robot.h
├── package.xml
└── src
├── action_control_01.cpp
├── action_robot_01.cpp
└── robot.cpp
3 directories, 6 files
这里我们添加配置文件：
CMakeList.txt
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(robot_control_interfaces REQUIRED)
find_package(rclcpp_action REQUIRED)
# action_robot节点
add_executable(action_robot_01
src/robot.cpp
src/action_robot_01.cpp
)
target_include_directories(action_robot_01 PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(action_robot_01 PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
ament_target_dependencies(
action_robot_01
"rclcpp"
"rclcpp_action"
"robot_control_interfaces"
)
install(TARGETS action_robot_01
DESTINATION lib/${PROJECT_NAME})
# action_control节点
add_executable(action_control_01
src/action_control_01.cpp
)
target_include_directories(action_control_01 PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(action_control_01 PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
ament_target_dependencies(
action_control_01
"rclcpp"
"rclcpp_action"
"robot_control_interfaces"
)
install(TARGETS action_control_01
DESTINATION lib/${PROJECT_NAME})
这里我们看到生成可执行文件的add_executable()当中似乎与我们之前写的有些不同，因为我们通过两个文件添加生成了一个节点
这是怎么回事？
如果你有幸读过《编译原理》，或许会明白，在上面这个多文件项目中，实际上是这样的结构：
- robot.h ← 类声明（接口）
- robot.cpp ← 类实现（定义）
- action_robot_01.cpp ← 使用 Robot 类的节点代码
CMake 本质上还是用来进行编译的，所以在这个过程中，跟我们一般的项目编译逻辑上是一致的，也就是说，如果你在上面的add_executable()中删掉了src/robot.cpp，那么:
- 编译器首先会读取 action_robot_01.cpp 的内容，发现了一行代码
#include "robot_demo_rclcpp/robot.h"，编译器回去寻找这个接口，发现里面的定义都已经被声明，但是具体的实现方法是没有定义的，所以就会爆出各种方法undefined的错误
机器人类的编写
这里还是先写后解释
robot.h:
#ifndef ROBOT_DEMO_RCLCPP_ROBOT_H_
#define ROBOT_DEMO_RCLCPP_ROBOT_H_
#include "rclcpp/rclcpp.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"
class Robot {
public:
Robot() = default;
~Robot() = default;
float move_step();
bool set_goal(float distance);
float get_current_pos();
int get_status();
bool close_goal();
void stop_move();
private:
float currrent_pose_ = 0.0;
float target_pose_ = 0.0;
float move_distance_ = 0.0;
std::atomic<bool> cancel_flag_{false};
int status_ MoveRobot::Feedback::STATUS_STOP;
};
#endif
这里定义的方法和成员都可以自己认识一下，回忆一下我们在哪里定义了一些看上去比较陌生的东西（MoveRobot.action）
一个需要注意的语句：
std::atomic<bool> cancel_flag_{false};这是创建了一个允许原子操作的flag，这里涉及线程安全的问题，在各种赋值和读取的过程中使用原子操作往往能保证更高的安全性和有序性，所以这也是相当有用的，这里先作为规范认识，目前你可以理解在我们需要从一个线程控制突然触发控制另一个线程的时候需要使用这样的操作防止指令乱序
这里我们还是默认各位不熟悉多文件编写模式，所以简单介绍一下
这个文件是.h的，作为头文件被编写，我们会在开头使用#ifndef来判断一个宏定义是否被定义，如果么有，我们就使用#define创建一个宏定义，并在结尾结束
这里声明一下使用宏定义的理由：
- 在我们的代码编写中，创建一个头文件
A.h现在我在编写B.h的时候使用了A.h的内容，然后又在C.cpp中使用了A.h和B.h，那么我的A.h就会在C.cpp中被定义两次，自然就会报错 - 所以我们需要使用宏定义来处理，这样当某个宏定义已经存在时，另一个文件就无法通过
#ifndef进行重复定义，所以这相当于一个保护机制
接下来说明宏的书写规范：
- 我们一般把所有
/这样的文件路径符号变成_，然后全部大写，把当前头文件在项目中的相对路径作为我们的宏的名称即可，结尾加上_H_或_H
接下来我们对机器人类定义的方法进行实现：
robot.cpp
#include "robot_demo_rclcpp/robot.h"
float Robot::move_step() {
int direct = move_distance_ / fabs(move_distance_);
float step = direct*fabs(target_pose_ - current_pose_) * 0.1;
current_pose_ += step;
return current_pose_;
}
bool Robot::set_goal(float distance) {
move_distance_ = distance;
target_pose_ += move_distance_;
if(close_goal()) {
status_ = MoveRobot::Feedback::STATUS_STOP;
return false;
}
status_ = MoveRobot::Feedback::STATUS_MOVING;
return true;
}
float Robot::get_current_pose() {return current_pose_;}
int Robot::get_status() {return status_;}
bool Robot::close_goal() {return fabs(target_pose_ - current_pose_)<0.01;}
void Robot::stop_move() {status_ = MoveRobot::Feedback::STATUS_STOP;}
编写机器人节点
action_robot_01.cpp
#include "robot_demo_rclcpp/robot.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"
class ActionRobot01 : public rclcpp::Node {
public:
using MoveRobot = robot_control_interfaces::action::MoveRobot;
using GoalHandleMoveRobot = rclcpp_action::ServerGoalHandle<MoveRobot>;
explicit ActionRobot01(std::string name) : Node(name) {
RCLCPP_INFO(this->get_logger(),"节点已启动：%s.",name.c_str());
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<MoveRobot>(this,"move_robot",
std:bind(&ActionRobot01::handle_goal,this,_1,_2),
std::bind(&ActionRobot01::handle_cancel,this,_1),
std::bind(ActionRobot01::handle_accepted,this,_1));
}
private:
Robot robot;
rclcpp_action::Server<MoveRobot>::SharedPtr action_server_;
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const MoveRobot::Goal> goal) {
RCLCPP_INFO(this->get_logger(),"Received goal request with distance %f",goal->distance);
(void)uuid;
if(fabs(goal->distance > 100)) {
RCLCPP_WARN(this->getlogger(),"目标距离太远了，本机器人表示拒绝！");
return rclcpp_action::GoalResponse::REJECT;
}
RCLCPP_INFO(this->get_logger(),"目标距离%f 准备出发！",goal->distance);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
RCLCPP_INFO(this->get_logger(),"Received request to cancel goal");
(void)goal_handle;
robot.stop_move();
return rclcpp_action::CancelResponse::ACCEPT;
}
void execute_move(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
const auto goal = goal_handle->get_goal();
RCLCPP_INFO(this->get_logger(),"开始执行移动 %f ...",goal->distance);
auto result = std::make_shared<MoveRobot::Result>();
rclcpp::Rate rate = rclcpp::Rate(2);
robot.set_goal(goal->distance);
while(rclcpp::ok() && !robot.close_goal()) {
robot.move_step();
auto feedback = std::make_shared<MoveRobot::Feedback>();
feedback->pose = robot.get_current_pose();
feedback->status = robot.get_status();
goal_handle->publish_feedback(feedback);
if(goal_handle->is_canceling()) {
result->pose = robot.get_current_pose();
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(),"Goal Canceled");
return;
}
RCLCPP_INFO(this->get_logger(),"Publisher Feedback");
rate.sleep();
}
result->pose = robot.get_current_pose();
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(),"Goal Succeeded");
}
void handle_accept(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
using std::placeholders::_1;
std::thread{std::bind(&ActionRobot01::execute_move,this,_1),goal_handle}.detach();
}
};
int main(int argc,char **argv) {
rclcpp::init(argc,argv);
auto action_server = std::make_shared<ActionRobot01>("action_robot_01");
rclcpp::spin(action_server);
rclcpp::shutdown();
return 0;
}
等等等！
怎么突然代码的信息量暴涨这么多？
事实上是因为这里添加了一个新的成员rclcpp_action这作为一个单独的包，内部先置了很多 Action 方法和参数，我们先在这里丢个文档
里面的方法和参数实在太多，这里不可能一个一个讲，但我会按照代码里出现的这些相关方法和参数为中心进行介绍
-
首先我们看到我们的构造函数前多了一个关键字：explicit，这个关键字代表在调用这个构造函数时被强制性使用显式构造
什么是显式构造？显式构造是相对于隐式构造来说的，显式构造的意思就是我已经设定好了这个函数的参数传递方法，不允许使用其他方法赋值或者传入类型不同的参数来调用我的函数，而隐式构造则可以用其它类型的参数通过类似于强制类型转换的模式进行参数传递，可能导致错误，并且会引起内存空间占用过多、无法释放等问题，影响维护，所以我们后面的代码中构造函数尽可能使用这个关键字来强制显式构造，更便于维护 -
rclcpp_action::create_server()这个函数主要创建了一个 action 当中的服务端，也就是任务的接收者和执行者，这里也可以看到除了this的自指针以及服务名称之外，一共有三个回调函数，三个回调函数的名称也能够看出来他们的功能了，分别表示处理接收到的目标请求、处理取消请求和处理已接受的目标，我们在下面还会继续细讲三个函数的触发条件以及他们的参数传递关系 -
处理目标请求回调函数：
rclcpp_action::GoalResponse handlegoal(const rclcpp_action:: GoalUUID& uuid,std::shared_ptr<const MoveRobot::Goal> goal)这个函数的返回类型为GoalResponse，这个类型主要有三种返回值：
-
REJECT：拒绝目标 -
ACCEPT_AND_EXECUTE：接受并立即执行 -
ACCEPT_AND_DEFER：接受但延迟执行当你打开源码看到这三个值的定义的时候，你会发现他们其实也不过是
enum了一下而已，不过这里还是需要注意代码规范，不要写数值，真的很丑我们继续看两个参数，其中一个是
uuid，类型是rclcpp _action中的GoalUUID，这个相当于对于目标设置的一个唯一标识符，可以用来调用目标的日志，后期看日志是很重要的，目前还用不到，知道概念就好另一个是
goal这是一个共享指针类型，它指向了我们通信接口里面的第一部分，也就是消息中Goal的内容
- 处理取消请求：
rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleMoveRobot> goal_handle)这个函数的返回类型也是一个enum类，当然也是经过封装的：
-
REJECT：拒绝取消请求 -
ACCEPT：接受取消请求这里的
GoalHandleMoveRobot是我们前面简化过的，事实上是rclcpp_action::ServerGoalHandle<MoveRobot>，这个类型定义的句柄是相当重要的，它是管理单个Action目标生命周期的重要句柄，其中他有很多方法，可以看下面的阉割版文档：
我们的代码中会出现很多关于句柄的方法，他是控制Action的核心之一，需要熟练掌握
当然，这个函数中没有用到，我们接着看下一个回调函数
-
处理已接受的目标：
void handle_accept(const std::shared_ptr<GoalHandleMoveRobot> goal_handle)我们可以看到这个函数是没有返回值的，而我们在这个函数中使用了std::thread{}.detach();创造了一个新的线程，在这个线程中执行函数，并使用.detach()将其从主线程中分离出去，这样新创建的线程就具有自己管理生命周期的能力，不会阻塞主线程继续响应其他消息请求，也就相当于创建了一个多线程的模式执行一个长周期的动作
这里挂一个图，简单理解一下.detch的玩法：
当然这里也是可以直接把
goal_handle填入占位符的，相当于直接绑定参数了，看上去更简洁，也是可以运行的 -
来到我们的
execute_move()函数，这里面把我们的目标句柄传入了，其实就相当于传入了这个 Action 任务的生命周期，我们可以看到里面使用了几种方法：
goal_handle->get_goal()表示读取了目标句柄中的值，也就是目标消息（客户端发送过来的目标请求，格式和我们创建的消息接口文件是一致的）；
goal_handle->publish_feedback(feedback);这里通过我们的Topic实时传回反馈数据，也就是我们的机器人到达的位置；
goal_handle->is_canceling()顾名思义，就是返回我们的 Action 任务是否被取消；
goal_handle->canceled(result);打上目标取消的标记，并且返回结果值；
goal_handle->succeed(result);在机器人跑完了之后返回已经顺利完成任务，返回当前所在的位置(result)
这样我们的机器人节点应该就很容易读懂了，相信各位此刻正醍醐灌顶，思路清晰
那么我们接下来继续讲解控制节点的讲解
编写机器人控制节点
action_control_01.cpp
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"
class ActionControl01 : public rclcpp:Node {
public:
using MoveRobot = robot_control_interfaces::action::MoveRobot;
using GoalHandleMoveRobot = rclcpp_action::ClientGoalHandle<MoveRobot>;
explicit ActionControl01(std::string name) : Node(name) {
RCLCPP_INFO(this->get_logger(),"节点已启动：%s.",name.c_str());
this->client_ptr_ = rclcpp::creat_client<MoveRobot>(this,"move_robot");
this->timer_ = create_wall_timer(std::chrono::milliseconds(500),std::bind(&ActionControl01::send_goal,this));
}
void send_goal() {
using namespace std::placeholders;
this->timer_->cancel();
if(!this->client_prt_->wait_for_action_server(std::chrono::seconds(10))) {
RCLCPP_ERROR(this->get_logger(),"Action server not available after waiting");
rclcpp::shutdown();
return;
}
auto goal_msg = MoveRobot::Goal();
goal_msg.distance = 10;
RCLCPP_INFO(this0>get_logger(),"Sending goal")l
auto send_goal_options = rclcpp_action::Client<MoveRobot>::SendGoalOptions();
send_goal_options.goal_response_callback = std::bind(&ActionControl01::goal_response_callback,this,_1);
send_goal_options.feedback_callback = std::bind(&ActionControl01::feedback_callback,this,_1,_2)
send_goal_options.result_callback = std::bind(&ActionControl01::result_callback,this,_1);
this->client_ptr_->async_send_goal(goal_msg,send_goal_options);
}
private:
rclcpp_action::Client<MoveRobot>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
void goal_response_callback(GoalHandleMoveRobot::SharedPtr goal_handle) {
if(!goal_handle) RCLCPP_ERROE(this->get_logger(),"Goal was rejected by server");
else RCLCPP_INFO(this->get_logger(),"Goal accepted by server,waiting for result");
}
void feed back_callback(GoalHandleMoveRobot::SharedPtr,const std::shared_ptr<const MoveRobot::Feedback> feedback) {
RCLCPP_INFO(this->get_logger(),"Feedback current pose:%f",feedback->pose);
}
void result_callback(const GoalHandleMoveRobot::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;
}
RCLCPP_INFO(this->get_logger(),"Result received: %f",result.result->pose);
}
};
int main(int argc,char ** argv) {
rclcpp::init(argc,argv);
auto action_client = std::make_shared<ActionControl01>("action_robot_cpp");
rclcpp::spin(action_client);
rclcpp::shutdown();
return 0;
}
上面的代码有一些没有见过的部分，下面我进行简单讲解：
-
我们发现在我们的构造函数中多了一个参数，那是我们的节点行为参数
Options，这个参数在这里不填也是可以的，目前作用并不大，所以先不进行更深入的解释 -
this->timer_->cancel()的意义是在定时器完成了定时请求的功能后就立即取消定时器，这样就实现了节点启动后定时发送一次请求的功能 -
下面的
wait_for_action_server是rclcpp_action::Client的一种方法，顾名思义，就是等待服务端上线了 -
这里的
rclcpp_action::Client<MoveRobot>::SendGoalOptions类型中，我们可以看到这个类型直接携带了三个回调函数，也就是对应我们目标发送出去之后需要查看的三个回调 -
在我们的
result_callback()的参数中，我们看到了类型GoalHandleMoveRobot::WrappedResult，这个类型是用于在 Action 客户端中抓取异步数据的，根据我们的接口文件生成的源码内容如下：struct WrappedResult { rclcpp_action::ResultCode code; MoveRobot::Result result; };第一个参数表示结构代码，有三种值：
- SUCCEEDED 执行成功
- ABORTED 执行异常终止
- CANCELED 执行被取消
这样我们就能看懂我们的代码了，接下来就可以编译测试了
Python
编写机器人类及控制节点
创建功能包和节点
这里我们不多重复，直接创建即可：
（接口文件的创建在这就不展示了，与上面是一样的）
cd Action
mkdir -p /action_py_ws/src
ros2 pkg create robot_demo_rclpy --build-type ament_python --dependencies rclpy robot_control_interfaces
cd example_action_rclpy/example_action_rclpy
touch action_robot_01.py
touch action_conrtol_01.py
touch robot.py
这里我们编写一下配置文件：
setup.py
entry_points={
'console_scripts': [
'action_robot_02 = example_action_rclpy.action_robot_02:main',
'action_control_02 = example_action_rclpy.action_control_02:main'
],
},
编写机器人类
from robot_control_interfaces.action import MoveRobot
import math
class Robot():
def __init__(self) -> None:
self.current_pose_ = 0.0
self.target_pose_ =0.0
self.move_distance_ = 0.0
self.status_ = MoveRobot.Feedback
def get_status(self):
return self.status_
def get_current_pose(self):
return self.current_pose_
def close_goal(self):
return math.fabs(self.target_pose_ - self.current_pose_) < 0.01
def stop_move(self):
self.status_ = MoveRobot.Feedback.STATUS_STOP
def move_step(self):
direct = self.move_distance_ / math.fabs(self.move_distance_)
step = direct * math.fabs(self.target_pose_ - self.current_pose_) * 0.1
self.current_pose_ += step # 移动一步
print(f"移动了：{step}当前位置：{self.current_pose_}")
return self.current_pose_
def set_goal(self, distance):
self.move_distance_ = distance
self.target_pose_ += distance
if self.close_goal():
self.stop_move()
return False
self.status_ = MoveRobot.Feedback.STATUS_MOVEING
return True
这里第一行的意思就是从 robot_control_interfaces 的 action 文件夹里导入一个叫 MoveRobot 的消息接口文件
其余部分的代码就相当简单了，不做过多解释
编写机器人节点
import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from rclpy.action.server import ServerGoalHandle
from robot_control_interfaces.action import MoveRobot
from robot_demo_rclpy.robot import Robot
class ActionRobot01(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info(f"节点已启动：{name}!")
self.robot_ = Robot()
self.action_server_ = ActionServer(
self, MoveRobot, 'move_robot', self.execute_callback
# ,callback_group=MutuallyExclusiveCallbackGroup()
)
def execute_callback(self, goal_handle: ServerGoalHandle):
self.get_logger().info('执行移动机器人')
feedback_msg = MoveRobot.Feedback()
self.robot_.set_goal(goal_handle.request.distance)
# rate = self.create_rate(2)
while rclpy.ok() and not self.robot_.close_goal():
# move
self.robot_.move_step()
# feedback
feedback_msg.pose = self.robot_.get_current_pose()
feedback_msg.status = self.robot_.get_status()
goal_handle.publish_feedback(feedback_msg)
# cancel check
if goal_handle.is_cancel_requested:
result = MoveRobot.Result()
result.pose = self.robot_.get_current_pose()
return result
time.sleep(0.5)
goal_handle.succeed()
result = MoveRobot.Result()
result.pose = self.robot_.get_current_pose()
return result
def main(args=None):
rclpy.init(args=args)
action_robot_01 = ActionRobot01("action_robot_01")
rclpy.spin(action_robot_01)
rclpy.shutdown()
我们会发现不同于 C++ 的 Action 代码，这里我们只写了一个回调函数，因为其他几个函数 Python 都已经默认给我们封装好了，所以这里只需要书写一个执行的回调即可
编写控制节点
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from robot_control_interfaces.action import MoveRobot
class ActionControl01(Node):
def __init__(self, name):
super().__init__(name)
self.get_logger().info(f"节点已启动：{name}!")
self.action_client_ = ActionClient(self, MoveRobot, 'move_robot')
self.send_goal_timer_ = self.create_timer(1, self.send_goal)
def send_goal(self):
self.send_goal_timer_.cancel()
goal_msg = MoveRobot.Goal()
goal_msg.distance = 5.0
self.action_client_.wait_for_server()
self._send_goal_future = self.action_client_.send_goal_async(goal_msg,
feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)')
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
result = future.result().result
self.get_logger().info(f'Result: {result.pose}')
def feedback_callback(self, feedback_msg):
feedback = feedback_msg.feedback
self.get_logger().info(f'Received feedback: {feedback.pose}')
def main(args=None):
rclpy.init(args=args)
action_robot_01 = ActionControl01("action_control_01")
rclpy.spin(action_robot_01)
rclpy.shutdown()
到这里，我们就可以顺利地编译测试了
补充
下面是之前说的封装好的几个函数：
def default_handle_accepted_callback(goal_handle):
"""Execute the goal."""
goal_handle.execute()
def default_goal_callback(goal_request):
"""Accept all goals."""
return GoalResponse.ACCEPT
def default_cancel_callback(cancel_request):
"""No cancellations."""
return CancelResponse.REJECT
浙公网安备 33010602011771号