0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看技术视频
  • 写文章/发帖/加入社区
会员中心
创作中心

完善资料让更多小伙伴认识你,还能领取20积分哦,立即完善>

3天内不再提示

机器人多节点动作通信编程方法

麦辣鸡腿堡 来源:古月居 作者:古月居 2023-11-27 17:10 次阅读

编程方法

相比之前话题和服务的程序,动作通信的例程相对较长,我们一起来运行并分析一下。

运行示例程序

图片

$ source /opt/tros/local_setup.bash
$ source install/local_setup.bash
$ ros2 run learning_action_cpp server 
$ ros2 run learning_action_cpp client

代码解析

动作的服务器fibonacci_action_server.cpp:

#include < inttypes.h >
#include < memory >
#include "learning_action_cpp/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"


class MinimalActionServer : public rclcpp::Node
{
public:
  using Fibonacci = learning_action_cpp::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle< Fibonacci >;


  explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("minimal_action_server", options)
{
    using namespace std::placeholders;


    this- >action_server_ = rclcpp_action::create_server< Fibonacci >(
      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< Fibonacci >::SharedPtr action_server_;


  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr< const Fibonacci::Goal > goal)
{
    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_ptr< GoalHandleFibonacci > goal_handle)
{
    RCLCPP_INFO(this- >get_logger(), "Received request to cancel goal");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }


  void execute(const std::shared_ptr< GoalHandleFibonacci > goal_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< Fibonacci::Feedback >();
    auto & sequence = feedback- >sequence;
    sequence.push_back(0);
    sequence.push_back(1);
    auto result = std::make_shared< Fibonacci::Result >();


    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_ptr< GoalHandleFibonacci > goal_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< MinimalActionServer >();


  rclcpp::spin(action_server);


  rclcpp::shutdown();
  return 0;
}

动作的客户端fibonacci_action_client.cpp:

#include < inttypes.h >
#include < memory >
#include < string >
#include < iostream >
#include "learning_action_cpp/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"


class MinimalActionClient : public rclcpp::Node
{
public:
  using Fibonacci = learning_action_cpp::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle< Fibonacci >;


  explicit MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
  : Node("minimal_action_client", node_options), goal_done_(false)
{
    this- >client_ptr_ = rclcpp_action::create_client< Fibonacci >(
      this- >get_node_base_interface(),
      this- >get_node_graph_interface(),
      this- >get_node_logging_interface(),
      this- >get_node_waitables_interface(),
      "fibonacci");


    this- >timer_ = this- >create_wall_timer(
      std::chrono::milliseconds(500),
      std::bind(&MinimalActionClient::send_goal, this));
  }


  bool is_goal_done() const
{
    return this- >goal_done_;
  }


  void send_goal()
{
    using namespace std::placeholders;


    this- >timer_- >cancel();


    this- >goal_done_ = false;


    if (!this- >client_ptr_) {
      RCLCPP_ERROR(this- >get_logger(), "Action client not initialized");
    }


    if (!this- >client_ptr_- >wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this- >get_logger(), "Action server not available after waiting");
      this- >goal_done_ = true;
      return;
    }


    auto goal_msg = Fibonacci::Goal();
    goal_msg.order = 10;


    RCLCPP_INFO(this- >get_logger(), "Sending goal");


    auto send_goal_options = rclcpp_action::Client< Fibonacci >::SendGoalOptions();
    send_goal_options.goal_response_callback =
      std::bind(&MinimalActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
      std::bind(&MinimalActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
      std::bind(&MinimalActionClient::result_callback, this, _1);
    auto goal_handle_future = this- >client_ptr_- >async_send_goal(goal_msg, send_goal_options);
  }


private:
  rclcpp_action::Client< Fibonacci >::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;
  bool goal_done_;


  void goal_response_callback(std::shared_future< GoalHandleFibonacci::SharedPtr > future)
{
    auto goal_handle = future.get();
    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(
    GoalHandleFibonacci::SharedPtr,
    const std::shared_ptr< const Fibonacci::Feedback > feedback)
{
    RCLCPP_INFO(
      this- >get_logger(),
      "Next number in sequence received: %" PRId32,
      feedback- >sequence.back());
  }


  void result_callback(const GoalHandleFibonacci::WrappedResult & result)
{
    this- >goal_done_ = true;
    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");
    for (auto number : result.result- >sequence) {
      RCLCPP_INFO(this- >get_logger(), "%" PRId32, number);
    }
  }
};  // class MinimalActionClient


int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto action_client = std::make_shared< MinimalActionClient >();


  while (!action_client- >is_goal_done()) {
    rclcpp::spin_some(action_client);
  }


  rclcpp::shutdown();
  return 0;
}
声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
  • 机器人
    +关注

    关注

    211

    文章

    28390

    浏览量

    206959
  • 通信
    +关注

    关注

    18

    文章

    6029

    浏览量

    135957
  • 程序
    +关注

    关注

    117

    文章

    3785

    浏览量

    81010
收藏 人收藏

    评论

    相关推荐

    机器人设计中PID控制的编程方法是什么?

    什么是PID控制?机器人设计中PID控制的编程方法是什么?
    发表于 06-30 06:53

    机器人编程的区别

    在前一篇文章中讲了机器人编程的区别,但总感觉讲的比较空泛,这篇文章继续讲讲那些区别。计算机编程机器人编程最大的区别就是一个是控制虚拟的,
    发表于 09-01 07:12

    机器人所用的线缆动作

    根据机器人动作选择电缆首先请掌握电缆的动作。配线时尽量加大弯曲半径,减少扭转和摩擦是减少断线故障的要点,然后根据该动作下的期望寿命,选择最合适的配线材料和配线
    发表于 06-09 10:44

    多节点大容量FPGA系统的远程升级方法

    多节点大容量FPGA系统的远程升级方法:针对目前广泛使用的以大容量FPGA 实现主要功能的多节点系统的远程升级问题,提出了一种基于ATmega64 单片机和RS485 总线以及接入以太网的主控
    发表于 11-20 17:42 17次下载

    多节点大容量FPGA系统的远程升级方法

    多节点大容量FPGA系统的远程升级方法 针对目前广泛使用的以大容量FPGA实现主要功能的多节点系统的远程升级问题,提出了一种基于ATmega64单片机和RS485总
    发表于 03-29 15:09 820次阅读
    <b class='flag-5'>多节点</b>大容量FPGA系统的远程升级<b class='flag-5'>方法</b>

    机器人多传感器测距系统研究与设计

    机器人多传感器测距系统研究与设计......
    发表于 12-23 14:46 10次下载

    机器人多传感器信息融合测距系统设计

    机器人多传感器信息融合测距系统设计....
    发表于 12-23 15:00 16次下载

    智能机器人多传感器融合感知方法

    智能机器人多传感器融合感知方法,感兴趣的小伙伴们可以瞧一瞧。
    发表于 09-20 16:10 19次下载

    解读机器人编程机器人语言

    机器人编程为使机器人完成某种任务而设置的动作顺序描述。机器人运动和作业的指令都是由程序进行控制,常见的编制
    的头像 发表于 11-24 15:32 6842次阅读

    机器人的最佳编程语言是什么?机器人十大流行编程语言汇总

    机器人的主要特点之一是其通用性,是机器人具有可编程能力是实现这一特点的重要手段。机器人编程必然涉及机器人
    的头像 发表于 05-01 17:32 3.5w次阅读

    常用的机器人编程方法有哪些

    机器人编程【robotprogramming】为使机器人完成某种任务而设置的动作顺序描述。机器人运动和作业的指令都是由程序进行控制,常见的编
    发表于 08-15 17:41 1.6w次阅读

    机器人编程是学的什么_机器人编程有什么前景

     机器人编程为使机器人完成某种任务而设置的动作顺序描述。机器人运动和作业的指令都是由程序进行控制,常见的编制
    的头像 发表于 07-23 11:42 3.8w次阅读

    基于区块链的智能机器人多传感信息加密控制方法

    基于区块链的智能机器人多传感信息加密控制方法
    发表于 06-23 10:44 5次下载

    机器人多节点话题通信模型介绍

    节点实现了机器人各种各样的功能,但这些功能并不是独立的,之间会有千丝万缕的联系,其中最重要的一种联系方式就是话题,它是节点间传递数据的桥梁。 通信模型 以两个
    的头像 发表于 11-27 17:25 488次阅读
    <b class='flag-5'>机器人多节点</b>话题<b class='flag-5'>通信</b>模型介绍

    机器人多节点话题通信编程方法

    编程方法 了解了话题的基本原理,接下来我们就要开始编写代码啦。 创建工作空间 请大家先按照这个流程创建工作空间、下载课程的例程代码,并进行编译。 $ mkdir –p dev_ws/src $ cd
    的头像 发表于 11-27 17:48 377次阅读
    <b class='flag-5'>机器人多节点</b>话题<b class='flag-5'>通信</b><b class='flag-5'>编程</b><b class='flag-5'>方法</b>