本系列教程作者:小鱼
公众号:鱼香ROS
QQ交流群:139707339
教学视频地址:小鱼的B站
完整文档地址:鱼香ROS官网
版权声明:如非允许禁止转载与商业用途。
因为后续课程使用到Action的地方都是在最后阶段,为了减轻大家学习负担,先不进行讲解,有兴趣的同学可以参考官方例程源码,小鱼这里也贴一份放在这。
#include #include#include "example_interfaces/action/fibonacci.hpp" #include "rclcpp/rclcpp.hpp" // TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp' #include "rclcpp_action/rclcpp_action.hpp" // 创建一个ActionServer类 class MinimalActionServer : public rclcpp::Node { public: using Fibonacci = example_interfaces::action::Fibonacci; using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle; explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("minimal_action_server", options) { using namespace std::placeholders; this->action_server_ = rclcpp_action::create_server( this->get_node_base_interface(), this->get_node_clock_interface(), this->get_node_logging_interface(), this->get_node_waitables_interface(), "fibonacci", std::bind(&MinimalActionServer::handle_goal, this, _1, _2), std::bind(&MinimalActionServer::handle_cancel, this, _1), std::bind(&MinimalActionServer::handle_accepted, this, _1)); } private: rclcpp_action::Server::SharedPtr action_server_; rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptrgoal) { RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); (void)uuid; // Let's reject sequences that are over 9000 if (goal->order > 9000) { return rclcpp_action::GoalResponse::REJECT; } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse handle_cancel( const std::shared_ptrgoal_handle) { RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; } void execute(const std::shared_ptrgoal_handle) { RCLCPP_INFO(this->get_logger(), "Executing goal"); rclcpp::Rate loop_rate(1); const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); auto & sequence = feedback->sequence; sequence.push_back(0); sequence.push_back(1); auto result = std::make_shared(); for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { // Check if there is a cancel request if (goal_handle->is_canceling()) { result->sequence = sequence; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Goal Canceled"); return; } // Update sequence sequence.push_back(sequence[i] + sequence[i - 1]); // Publish feedback goal_handle->publish_feedback(feedback); RCLCPP_INFO(this->get_logger(), "Publish Feedback"); loop_rate.sleep(); } // Check if goal is done if (rclcpp::ok()) { result->sequence = sequence; goal_handle->succeed(result); RCLCPP_INFO(this->get_logger(), "Goal Succeeded"); } } void handle_accepted(const std::shared_ptrgoal_handle) { using namespace std::placeholders; // this needs to return quickly to avoid blocking the executor, so spin up a new thread std::thread{std::bind(&MinimalActionServer::execute, this, _1), goal_handle}.detach(); } }; // class MinimalActionServer int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto action_server = std::make_shared(); rclcpp::spin(action_server); rclcpp::shutdown(); return 0; }作者介绍:
我是小鱼,机器人领域资深玩家,现深圳某独脚兽机器人算法工程师一枚 初中学习编程,高中开始接触机器人,大学期间打机器人相关比赛实现月入2W+(比赛奖金) 目前在输出机器人学习指南、论文注解、工作经验,欢迎大家关注小智,一起交流技术,学习机器人