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

source install/setup.bash

ros2 run example_action_rclcpp action_control_01

相关推荐
只做开心事40 分钟前
C++之红黑树模拟实现
开发语言·c++
程序员老冯头2 小时前
第十五章 C++ 数组
开发语言·c++·算法
程序猿会指北3 小时前
【鸿蒙(HarmonyOS)性能优化指南】启动分析工具Launch Profiler
c++·性能优化·harmonyos·openharmony·arkui·启动优化·鸿蒙开发
无 证明7 小时前
new 分配空间;引用
数据结构·c++
别NULL11 小时前
机试题——疯长的草
数据结构·c++·算法
CYBEREXP200812 小时前
MacOS M3源代码编译Qt6.8.1
c++·qt·macos
yuanbenshidiaos12 小时前
c++------------------函数
开发语言·c++
yuanbenshidiaos12 小时前
C++----------函数的调用机制
java·c++·算法
tianmu_sama13 小时前
[Effective C++]条款38-39 复合和private继承
开发语言·c++
羚羊角uou13 小时前
【C++】优先级队列以及仿函数
开发语言·c++