ROS2- Action的实现cpp+py-15
ROS2中的action通信
ROS2中四大通信利器中话题、服务、参数这三个,
最后一个Action。
话题topic适用于节点间单向的频繁的数据传输
服务service则适用于节点间双向的数据传递
参数param则用于动态调整节点的设置
通过服务service的方式进行通信，发送一个目标点给机器人，让机器人移动到该点的场景
不能确认服务端是否接收并处理请求
假设机器人收到了请求，但是依然不知道机器人此时的位置和距离目标点的距离（没有反馈）
假设机器人移动一半，你想让机器人停下来，也没有办法通知机器人
上面的场景在机器人控制当中经常出现，
控制导航程序，
控制机械臂运动，
控制小乌龟旋转等，
很显然单个话题和服务不能满足，
ROS2针对控制这一场景，基于原有的话题和服务，设计了动作（Action）这一通信方式来解决这一问题。
Action的组成部分
目标：Action客户端告诉服务端要做什么，服务端针对该目标要有响应
反馈：Action服务端告诉客户端此时做的进度如何（类似与工作汇报）。
结果：Action服务端最终告诉客户端其执行结果。
参数param是由服务service构建出来的，
Action是由话题topic和服务service共同构建出来的（一个Action=三个服务+两个话题）
三个服务分别是：1.目标传递服务 2.结果传递服务 3.取消执行服务
两个话题：1.反馈话题（服务发布，客户端订阅） 2.状态话题（服务端发布，客户端订阅）
乌龟模拟器
ros2 run turtlesim turtlesim_node
键盘控制节点
ros2 run turtlesim turtle_teleop_key
Use arrow keys to move the turtle.
Use G|B|V|C|D|E|R|T keys to rotate to absolute orientations. 'F' to cancel a rotation.
在小乌龟的遥控窗口使用键盘上的F按键周围的按键来尝试运行控制下小乌龟的方向，
看到小乌龟根据我们所按下按键所在的方向来在原地进行旋转。
ros2 action list -t # -t 看到action的类型
/turtle1/rotate_absolute [turtlesim/action/RotateAbsolute]
参看接口类型
ros2 interface show turtlesim/action/RotateAbsolute
# The desired heading in radians
float32 theta
---
# The angular displacement in radians to the starting position
float32 delta
---
# The remaining rotation in radians
float32 remaining
ros2 action info /turtle1/rotate_absolute
Action: /turtle1/rotate_absolute
Action clients: 1
/teleop_turtle
Action servers: 1
/turtlesim
发送actin请求到服务端
ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 1.6}"
Waiting for an action server to become available...
Sending goal:
theta: 1.6
Goal accepted with ID: b215ad060899444793229171e76481c7
Result:
delta: -1.5840003490447998
添加feedback
ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 1.5}" --feedback
Waiting for an action server to become available...
Sending goal:
theta: 1.5
Feedback:
remaining: -0.0840003490447998
Goal accepted with ID: b368de0ed1a54e00890f1b078f4671c8
Feedback:
remaining: -0.06800031661987305
Feedback:
remaining: -0.05200028419494629
Feedback:
remaining: -0.03600025177001953
Result:
delta: 0.08000016212463379
Goal finished with status: SUCCEEDED
控制机器人移动到点
cd chapt4_ws/
ros2 pkg create robot_control_interfaces --build-type ament_cmake --destination-directory src
mkdir -p src/robot_control_interfaces/action
touch src/robot_control_interfaces/action/MoveRobot.action
# Goal: 要移动的距离
float32 distance
---
# Result: 最终的位置
float32 pose
---
# Feedback: 中间反馈的位置和状态
float32 pose
uint32 status
uint32 STATUS_MOVEING = 3
uint32 STATUS_STOP = 4
packages.xml
<depend>rosidl_default_generators</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
CMakeLists.txt
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/MoveRobot.action"
)
colcon build --packages-select robot_control_interfaces
编译成功后，可看到C++头文件和Python相关文件
C++ Action消息头文件目录install/robot_control_interfaces/include
Python Action消息文件目录install/robot_control_interfaces/local/lib/python3.10
创建example_action_rclcpp功能包，
添加robot_control_interfaces、rclcpp_action、rclcpp三个依赖，
自动创建action_robot_01节点，并手动创建action_control_01.cpp节点。
cd chapt4_ws/
ros2 pkg create example_action_rclcpp --build-type ament_cmake --dependencies rclcpp rclcpp_action robot_control_interfaces --destination-directory src --node-name action_robot_01
touch src/example_action_rclcpp/src/action_control_01.cpp
创建Robot类的头文件和CPP文件
touch src/example_action_rclcpp/include/example_action_rclcpp/robot.h
touch src/example_action_rclcpp/src/robot.cpp
tree src/example_action_rclcpp/ -L 3
目录
.
├── CMakeLists.txt
├── include
│ └── example_action_rclcpp
│ └── robot.h
├── package.xml
└── src
├── action_control_01.cpp
├── action_robot_01.cpp
└── robot.cpp
vi src/example_action_rclcpp/include/example_action_rclcpp/robot.h
/*
copyright
*/
#ifndef EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#define EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#include "rclcpp/rclcpp.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"
class Robot {
public:
Robot() = default;
~Robot() = default;
private:
};
#endif // EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
vi src/example_action_rclcpp/src/robot.cpp
#include "example_action_rclcpp/robot.h"
/*移动一小步，请间隔500ms调用一次*/
float Robot::move_step() {
int direct = move_distance_ / fabs(move_distance_);
float step = direct * fabs(target_pose_ - current_pose_) *
0.1; /* 每一步移动当前到目标距离的1/10*/
current_pose_ += step;
std::cout << "移动了：" << step << "当前位置：" << current_pose_ << std::endl;
return current_pose_;
}
/*移动一段距离*/
bool Robot::set_goal(float distance) {
move_distance_ = distance;
target_pose_ += move_distance_;
/* 当目标距离和当前距离大于0.01同意向目标移动 */
if (close_goal()) {
status_ = MoveRobot::Feedback::STATUS_STOP;
return false;
}
status_ = MoveRobot::Feedback::STATUS_MOVEING;
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;
} /*停止移动*/
vi src/example_action_rclcpp/src/action_robot_01.cpp
#include "example_action_rclcpp/robot.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"
// 创建一个ActionServer类
class ActionRobot01 : public rclcpp::Node {
public:
explicit ActionRobot01(std::string name) : Node(name) {
RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
}
};
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;
}
vi src/example_action_rclcpp/src/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:
explicit ActionControl01(std::string name): Node(name) {
RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
}
}; // class ActionControl01
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;
}
vi src/example_action_rclcpp/CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(example_action_rclcpp)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
ind_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(robot_control_interfaces REQUIRED)
find_package(example_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"
"example_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"
"example_interfaces"
)
install(TARGETS action_control_01
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package() # 这个不能少呀
修改robot.h
vi src/example_action_rclcpp/include/example_action_rclcpp/robot.h
#ifndef EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#define EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#include "rclcpp/rclcpp.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"
class Robot {
public:
using MoveRobot = robot_control_interfaces::action::MoveRobot;
Robot() = default;
~Robot() = default;
float move_step(); /*移动一小步，请间隔500ms调用一次*/
bool set_goal(float distance); /*移动一段距离*/
float get_current_pose();
int get_status();
bool close_goal(); /*是否接近目标*/
void stop_move(); /*停止移动*/
private:
float current_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 // EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
编写机器人节点
vi src/example_action_rclcpp/src/action_robot_01.cpp
替换ActionRobot01类
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; // NOLINT
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->get_logger(), "目标距离太远了，本机器人表示拒绝！");
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(), "Publish Feedback"); /*Publish feedback*/
rate.sleep();
}
result->pose = robot.get_current_pose();
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal Succeeded");
}
void handle_accepted(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
using std::placeholders::_1;
std::thread{std::bind(&ActionRobot01::execute_move, this, _1), goal_handle}
.detach();
}
};
以上的代码都是围绕着Action展开
创建Action的API https://docs.ros2.org/latest/api/rclcpp_action/
Action使用了三个回调函数:
handle_goal，收到目标，反馈是否可以执行该目标，可以则返回ACCEPT_AND_EXECUTE,不可以则返回REJECT
handle_cancel，收到取消运行请求，可以则返回ACCEPT，不可以返回REJECT
handle_accepted，处理接受请求，当handle_goal中对移动请求ACCEPT后则进入到这里进行执行，这里是单独开了个线程进行执行execute_move函数，目的是避免阻塞主线程。
执行函数execute_move，调用机器人，进行一步步的移动，用了while循环的形式，
不断调用机器人移动并获取机器人的位置，通过feedback进行反馈。
同时检测任务是否被取消，如顺利执行完成则反馈最终结果。
还用到了Rate函数来精准控制循环的周期，让其保持为2HZ
客户端，发送请求的时候可以指定三个回调函数：
goal_response_callback，目标的响应回调函数。
feedback_callback，执行过程中进度反馈接收回调函数。
result_callback，最终结果接收的回调函数。
利用定时器完成了定时请求的功能，请求一次后就立马使用timer_->cancel();
取消掉了这个定时器，如此就实现了节点启动后定时发一次请求的功能。
vi src/example_action_rclcpp/src/action_control_01.cpp
控制节点ActionControl01 类代码替换
class ActionControl01 : public rclcpp::Node {
public:
using MoveRobot = robot_control_interfaces::action::MoveRobot;
using GoalHandleMoveRobot = rclcpp_action::ClientGoalHandle<MoveRobot>;
explicit ActionControl01(
std::string name,
const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions())
: Node(name, node_options) {
RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
this->client_ptr_ =
rclcpp_action::create_client<MoveRobot>(this, "move_robot");
this->timer_ =
this->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_ptr_->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(this->get_logger(), "Sending goal");
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_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(
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);
// rclcpp::shutdown();
}
}; // class ActionControl01
cd chapt4_ws/
colcon build --packages-up-to example_action_rclcpp
source install/setup.bash
ros2 run example_action_rclcpp action_robot_01
新的终端
source install/setup.bash
ros2 run example_action_rclcpp action_control_01
日志
ros2 run example_action_rclcpp action_robot_01
[INFO] [1749562409.160881311] [action_robot_01]: 节点已启动：action_robot_01.
[INFO] [1749562417.859617940] [action_robot_01]: Received goal request with distance 10.000000
[INFO] [1749562417.859734151] [action_robot_01]: 目标距离10.000000我可以走到，本机器人接受，准备出发！
[INFO] [1749562417.860185315] [action_robot_01]: 开始执行移动 10.000000 。。。
移动了：1当前位置：1
[INFO] [1749562417.860542695] [action_robot_01]: Publish Feedback
移动了：0.348678当前位置：6.86189
[INFO] [1749562422.860494849] [action_robot_01]: Publish Feedback
移动了：0.313811当前位置：7.1757
[INFO] [1749562423.360542041] [action_robot_01]: Publish Feedback
移动了：0.28243当前位置：7.45813
[INFO] [1749562423.860495917] [action_robot_01]: Publish Feedback
移动了：0.254187当前位置：7.71232
[INFO] [1749562424.360582613] [action_robot_01]: Publish Feedback
移动了：0.228768当前位置：7.94109
移动了：0.00785513当前位置：9.9293
[INFO] [1749562440.860589286] [action_robot_01]: Publish Feedback
移动了：0.00706959当前位置：9.93637
[INFO] [1749562449.360526684] [action_robot_01]: Publish Feedback
移动了：0.00117893当前位置：9.98939
[INFO] [1749562449.860600216] [action_robot_01]: Publish Feedback
移动了：0.00106106当前位置：9.99045
[INFO] [1749562450.360557620] [action_robot_01]: Publish Feedback
[INFO] [1749562450.860663933] [action_robot_01]: Goal Succeeded
[INFO] [1749562495.135887672] [rclcpp]: signal_handler(signum=2)
[INFO] [1749562417.353954569] [action_robot_cpp]: 节点已启动：action_robot_cpp.
[INFO] [1749562417.859185677] [action_robot_cpp]: Sending goal
[INFO] [1749562417.860009661] [action_robot_cpp]: Goal accepted by server, waiting for result
[INFO] [1749562417.860599894] [action_robot_cpp]: Feedback current pose:1.000000
[INFO] [1749562418.360591052] [action_robot_cpp]: Feedback current pose:1.900000
[INFO] [1749562430.860612878] [action_robot_cpp]: Feedback current pose:9.418503
...
[INFO] [1749562448.860650902] [action_robot_cpp]: Feedback current pose:9.986900
[INFO] [1749562449.360610597] [action_robot_cpp]: Feedback current pose:9.988211
[INFO] [1749562449.860671861] [action_robot_cpp]: Feedback current pose:9.989389
[INFO] [1749562450.360640559] [action_robot_cpp]: Feedback current pose:9.990451
[INFO] [1749562450.860796473] [action_robot_cpp]: Result received: 9.990451
^C[INFO] [1749562497.130975137] [rclcpp]: signal_handler(signum=2)
可以尝试再编写一个定时器，在节点启动的第5s时发送取消执行请求，看看是否可以让机器人停下来
python实现
cd chapt4_ws/
ros2 pkg create example_action_rclpy --build-type ament_python --dependencies rclpy robot_control_interfaces --destination-directory src --node-name action_robot_02
手动再创建action_control_02节点文件
touch src/example_action_rclpy/example_action_rclpy/action_control_02.py
手动创建机器人类robot.py
touch src/example_action_rclpy/example_action_rclpy/robot.py
robot.py
from robot_control_interfaces.action import MoveRobot
import math
class Robot():
"""机器人类，模拟一个机器人"""
def __init__(self) -> None:
pass
def get_status(self):
"""获取状态"""
pass
def get_current_pose(self):
"""获取当前位置"""
pass
def close_goal(self):
"""接近目标"""
pass
def stop_move(self):
"""停止移动"""
pass
def move_step(self):
"""移动一小步"""
pass
def set_goal(self, distance):
"""设置目标"""
pass
action_robot_02.py
#!/usr/bin/env python3
import time
# 导入rclpy相关库
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 example_action_rclpy.robot import Robot
#from rclpy.executors import MultiThreadedExecutor
#from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
class ActionRobot02(Node):
"""机器人端Action服务"""
def __init__(self,name):
super().__init__(name)
self.get_logger().info(f"节点已启动：{name}!")
def main(args=None):
"""主函数"""
rclpy.init(args=args)
action_robot_02 = ActionRobot02("action_robot_02")
# 采用多线程执行器解决rate死锁问题
# executor = MultiThreadedExecutor()
# executor.add_node(action_robot_02)
# executor.spin()
rclpy.spin(action_robot_02)
rclpy.shutdown()
action_control_02.py
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
# 导入Action接口
from robot_control_interfaces.action import MoveRobot
class ActionControl02(Node):
"""Action客户端"""
def __init__(self,name):
super().__init__(name)
self.get_logger().info(f"节点已启动：{name}!")
def main(args=None):
"""主函数"""
rclpy.init(args=args)
action_robot_02 = ActionControl02("action_control_02")
rclpy.spin(action_robot_02)
rclpy.shutdown()
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'
],
},
机器人 class类
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
机器人节点 class
class ActionRobot02(Node):
"""机器人端Action服务"""
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):
"""执行回调函数,若采用默认handle_goal函数则会自动调用"""
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
# rate.sleep() # Rate会造成死锁，单线程执行器时不能使用
time.sleep(0.5)
goal_handle.succeed()
result = MoveRobot.Result()
result.pose = self.robot_.get_current_pose()
return result
控制节点class
class ActionControl02(Node):
"""Action客户端"""
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}')
控制节点依然采用三个回调函数实现数据的接收
goal_response_callback，收到目标处理结果。
get_result_callback，获取结果反馈。
feedback_callback，接收过程信息。
cd chapt4_ws/
colcon build --packages-up-to example_action_rclpy
运行机器人节点
source install/setup.bash
ros2 run example_action_rclpy action_robot_02
新终端
source install/setup.bash
ros2 run example_action_rclpy action_control_02
使用Python编写Action，在设计上Python显得比C++更好用些
浙公网安备 33010602011771号