ROS2使用C++开发动作通信

1.开发接口节点

cd chapt4_ws/

ros2 pkg create robot_control_interfaces --build-type ament_cmake --destination-directory src --maintainer-name "joe" --maintainer-email "1027038527@qq.com"

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

编辑package.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"
)

2.开发动作节点

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 --maintainer-name "joe" --maintainer-email "1027038527@qq.com"

开发控制节点
touch 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:
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

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;
}

开发机器人节点

#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:
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();
}
};

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;
}

编译、运行

source install/setup.bash
ros2 run example_action_rclcpp action_robot_01