Action通信 实践案例

news2024/9/20 20:16:32

Action通信 Server端实现

创建服务端功能包（注意目录层级）：

ros2 pkg create my_exer05_action_server --build-type ament_cmake --node-name nav_server --dependencies rclcpp rclcpp_action my_exer_interfaces nav_msgs

 

/*
    需求：简单的模拟导航功能，向机器人发送一个前进N米的请求，机器人就会以 0.1m/s的速度前进，
         当与目标点的距离小于0.05m时，机器人就会停止运动，返回机器人的停止坐标，并且在此过程中，
         会连续反馈机器人与目标点之间的剩余距离。
    流程：
        1.包含头文件；
        2.初始化ROS2客户端；
        3.自定义节点类；
          3-1.创建参数服务客户端，连接到速度发布节点；
          3-2.创建订阅方，订阅机器人的里程计以获取机器人坐标；
          3-3.创建动作服务端，解析客户端的相关并生成响应。
        4.调用spin函数，并传入节点对象指针；
        5.资源释放。
*/
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "exer_interfaces/action/nav.hpp"

using namespace std::chrono_literals;
using namespace std::placeholders;
// 3.自定义节点类；
class NavServer: public rclcpp::Node{
public:
    NavServer():Node("nav_server_node_cpp"){
        last_x = 0.0;
        last_y = 0.0;
        // 3-1.创建参数服务客户端，连接到速度发布节点；
        paramClient = std::make_shared<rclcpp::AsyncParametersClient>(this,"pub_vel_node_cpp");
        // 等待服务连接
        while (!paramClient->wait_for_service(1s))
        {
            if (!rclcpp::ok())
            {
              return;
            }  
            RCLCPP_INFO(this->get_logger(),"服务未连接");
        }
        //
        RCLCPP_INFO(this->get_logger(),"已经连接成功速度发送节点的参数服务，可以设置线速度和角速度了");
        // 3-2.创建订阅方，订阅机器人的里程计以获取机器人坐标；
        sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>("odom",10,std::bind(&NavServer::on_timer,this,_1));
        // 3-3.创建动作服务端，解析客户端的相关并生成响应。
        nav_action_server_ = rclcpp_action::create_server<exer_interfaces::action::Nav>(
          this,
          "nav",
          std::bind(&NavServer::handle_goal,this,_1,_2),
          std::bind(&NavServer::handle_cancel,this,_1),
          std::bind(&NavServer::handle_accepted,this,_1)
        );
    }
private:
    rclcpp::AsyncParametersClient::SharedPtr paramClient;
    double x,y; // 当前机器人坐标
    double last_x,last_y; // 每次导航时，机器人起点坐标
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
    rclcpp_action::Server<exer_interfaces::action::Nav>::SharedPtr nav_action_server_;

    void on_timer(const nav_msgs::msg::Odometry & odom){
        x = odom.pose.pose.position.x;
        y = odom.pose.pose.position.y;
    }

    // 解析动作客户端发送的请求；
    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & goal_uuid, std::shared_ptr<const exer_interfaces::action::Nav::Goal> goal){
        (void)goal_uuid;
        float x = goal->goal;
        if (x > 0.0)
        {
          RCLCPP_INFO(this->get_logger(),"请求前进%.2f米", x);
        } else {
          RCLCPP_INFO(this->get_logger(),"只许进，不许退!");
          return rclcpp_action::GoalResponse::REJECT;
        }
        paramClient->set_parameters({rclcpp::Parameter("linear",0.1),rclcpp::Parameter("angular",0.0)});
        
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    // 处理动作客户端发送的取消请求；
    rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<rclcpp_action::ServerGoalHandle<exer_interfaces::action::Nav>> goal_handle){
        (void)goal_handle;
        RCLCPP_INFO(this->get_logger(),"任务取消中....!");
        paramClient->set_parameters({rclcpp::Parameter("linear",0.0),rclcpp::Parameter("angular",0.0)});

        return rclcpp_action::CancelResponse::ACCEPT;
    }
    void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<exer_interfaces::action::Nav>> goal_handle){
        RCLCPP_INFO(this->get_logger(),"开始执行任务......");
        // 获取目标点
        float goal = goal_handle->get_goal()->goal;
        // 连续反馈
        auto feedback = std::make_shared<exer_interfaces::action::Nav::Feedback>();
        // 最终结果
        auto result = std::make_shared<exer_interfaces::action::Nav::Result>();

        // 设置连续反馈
        rclcpp::Rate rate(1.0);
        while(rclcpp::ok()){
          // 检查任务是否被取消；
          if(goal_handle->is_canceling()){
            result->x = x;
            result->y = y;
            goal_handle->canceled(result);
            RCLCPP_INFO(this->get_logger(), "任务取消");
            last_x = x;
            last_y = y;
            paramClient->set_parameters({rclcpp::Parameter("linear",0.0),rclcpp::Parameter("angular",0.0)});
            return;
          }   

          double distance = goal - (x - last_x);
          feedback->distance = distance;
          goal_handle->publish_feedback(feedback);

          if (distance <= 0.1)
          {
            break;
          }
          
          rate.sleep();
        }
        // 设置最终结果
        if (rclcpp::ok()) {
          result->x = x;
          result->y = y;
          last_x = x;
          last_y = y;
          goal_handle->succeed(result);
          paramClient->set_parameters({rclcpp::Parameter("linear",0.0),rclcpp::Parameter("angular",0.0)});
          RCLCPP_INFO(this->get_logger(), "任务完成！");
        }
    }
    // 创建新线程处理请求；
    void handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<exer_interfaces::action::Nav>> goal_handle){
        std::thread{std::bind(&NavServer::execute,this,_1),goal_handle}.detach();
    }

};
int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc,argv);
    // 4.调用spain函数，并传入节点对象指针；
    rclcpp::spin(std::make_shared<NavServer>());
    // 5.资源释放。
    rclcpp::shutdown();
    return 0;
}

案例4：

-------------运行动作服务端 测试-----------

进入ws03_exercise目录下

ros2 launch stage_ros2 my_house.launch.py    // 启动 stage仿真环境
ros2 run my_exer01_topic_pub pub_vel         // 启动速度发布方
ros2 run my_exer05_action_server nav_server  // 启动Action服务端
ros2 action send_goal /nav my_exer_interfaces/action/Nav "{'goal': 10.0}" -f // 命令行方式，传递目标点 前进10m

Action通信 Client端实现

创建客户端功能包（注意目录层级）：

ros2 pkg create my_exer06_action_client --build-type ament_cmake --node-name nav_client --dependencies rclcpp rclcpp_action my_exer_interfaces

 

/*
  需求：向服务端提交数据（浮点类型前进距离），并处理响应
  流程：
    1. 包含头文件；
    2. 初始化 ROS2 客户端；
    3. 处理终端提交的数据； 
    4. 自定义节点类；
      4-1.创建动作通信客户端
      4-2.编写发送请求函数
      4-3.处理服务端返回的“请求数据是否合法”
      4-4.处理服务端的连续反馈
      4-5.处理最终响应
      4-6.特定条件下发送取消请求。
        a.条件：客户端按下ctrl + c   rclcpp::ok()
        b.发送取消任务请求  action_client_->async_cancel_goal(goal_handle)
    5. 调用自定义节点类对象向服务端发送数据；
    6. 调用 spin 函数，并传入节点对象指针；
    7. 释放资源；
*/
// 包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "my_exer_interfaces/action/nav.hpp"

using namespace std::placeholders;
using namespace std::chrono_literals;
using my_exer_interfaces::action::Nav;

// 自定义节点类；
class NavClient : public rclcpp::Node{
public:
    NavClient(): Node("nav_client_node_cpp"){
        RCLCPP_INFO(this->get_logger(), "NavClient创建！}");
      // 4-1.创建动作通信客户端
      action_client_ = rclcpp_action::create_client<Nav>(this,"/nav");
    }
    // 4-2.编写发送请求函数
    void send_goal(double goal){
        // 1.保证可以连接到服务端
        if(!action_client_->wait_for_action_server(10s))
        {
            RCLCPP_ERROR(this->get_logger(),"连接超时！");
            return;
        }
        RCLCPP_INFO(this->get_logger(),"连接成功！");
        // 2.组织被发送的请求对象
        auto request = Nav::Goal();
        request.goal = goal;
        // 3.封装回调函数
        rclcpp_action::Client<Nav>::SendGoalOptions options;

        // std::function<void (typename GoalHandle::SharedPtr)>;
        options.goal_response_callback = std::bind(&NavClient::goal_response_callback,this,_1);

        // std::function<void (typename ClientGoalHandle<ActionT>::SharedPtr,const std::shared_ptr<const Feedback>)>;
        options.feedback_callback = std::bind(&NavClient::feedback_callback,this,_1,_2);

        // std::function<void (const WrappedResult & result)>;
        options.result_callback = std::bind(&NavClient::result_callback,this,_1);


        // 4.发送
        action_client_->async_send_goal(request, options); // 核心API

    }
private:
    rclcpp_action::Client<Nav>::SharedPtr action_client_;
      // 4-3.处理服务端返回的“请求数据是否合法”
      void goal_response_callback(rclcpp_action::ClientGoalHandle<Nav>::SharedPtr goal_handle){
          if(!goal_handle){
              RCLCPP_ERROR(this->get_logger(),"请求数据非法，请先检查提交的是否是一个正数。");
          }else{
              RCLCPP_INFO(this->get_logger(),"请求数据被受理！");
              // 单独一根线程，循环判断终端状态
              std::thread{std::bind(&NavClient::cancel, this,_1), goal_handle}.detach();
          }
      }
      void cancel(rclcpp_action::ClientGoalHandle<Nav>::SharedPtr goal_handle){
          // 当rclcpp::ok() 返回 false 时，表示终端被按下了 ctrl + c , 取消任务
          while(rclcpp::ok()){}
          action_client_->async_cancel_goal(goal_handle);
      }


      // 4-4.处理服务端的连续反馈
      void feedback_callback(rclcpp_action::ClientGoalHandle<Nav>::SharedPtr goal_handle,const std::shared_ptr<const Nav::Feedback> feedback){
          // 直接将机器人和目标点之间的剩余距离输出
          (void)goal_handle;
          RCLCPP_INFO(this->get_logger(),"剩余距离：%.2f",feedback->distance);
      }
     // 4-5.处理最终响应
     void result_callback(const rclcpp_action::ClientGoalHandle<Nav>::WrappedResult & result){
        // 输出机器人的停止坐标
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
            RCLCPP_INFO(this->get_logger(),"导航成功！");
            RCLCPP_INFO(this->get_logger(),"机器人停止的坐标：(%.2f,%.2f)",result.result->x,result.result->y);
        }else{
            RCLCPP_ERROR(this->get_logger(),"机器人没有到达目标点！");
        }
        rclcpp::shutdown();
     }
};

int main(int argc, char * argv[]){
    // 2.初始化 ROS2 客户端；
    rclcpp::init(argc, argv);
    // 3.处理终端提交的数据；
    if(argc != 2){
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"请提交一个距离数据！");
      return 1;
    }
    auto nav_client = std::make_shared<NavClient>();
    // 5.调用自定义节点类对象向服务端发送数据
    nav_client->send_goal(atof(argv[1]));
    // 6.调用 spin 函数，并传入节点对象指针。
    rclcpp::spin(nav_client);
    // 7.释放资源；
    rclcpp::shutdown();
    return 0;
}

两种测试方案

1. 用stage仿真环境测试

 

 2.用实体机器人测试

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1943763.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

【vim】ubuntu20-server 安装配置 vim 最新最详细

【vim】ubuntu20-server 安装配置 vim 最新最详细

在编程的艺术世界里，代码和灵感需要寻找到最佳的交融点，才能打造出令人为之惊叹的作品。而在这座秋知叶i博客的殿堂里，我们将共同追寻这种完美结合，为未来的世界留下属于我们的独特印记。【vim】ubuntu20-server 安装配置 vim 最新最详细 开发环境一、vim github二、安装必…
阅读更多...
基于SpringBoot实现验证码功能

基于SpringBoot实现验证码功能

目录 一 实现思路 二 代码实现 三 代码汇总 现在的登录都需要输入验证码用来检测是否是真人登录&#xff0c;所以验证码功能在现在是非常普遍的&#xff0c;那么接下来我们就基于springboot来实现验证码功能。 一 实现思路 今天我们介绍的是两种主流的验证码&#xff0c;一…
阅读更多...
IP地址专用SSL/https证书——10分钟签发

IP地址专用SSL/https证书——10分钟签发

一般常用的SSL证书多为域名型SSL证书&#xff0c;即需要提供准确的域名。如果不能提供域名&#xff0c;只能提供IP地址&#xff0c;则需要一种特殊的SSL证书——IP地址证书。下面是IP地址证书的申请教程 IP地址专用SSL证书获取链接https://www.joyssl.com/certificate/select/…
阅读更多...
SQL中的LEFT JOIN、RIGHT JOIN和INNER JOIN

SQL中的LEFT JOIN、RIGHT JOIN和INNER JOIN

在SQL中&#xff0c;JOIN操作是连接两个或多个数据库表&#xff0c;并根据两个表之间的共同列&#xff08;通常是主键和外键&#xff09;返回数据的重要方法。其中&#xff0c;LEFT JOIN&#xff08;左连接&#xff09;、RIGHT JOIN&#xff08;右连接&#xff09;和INNER JOIN…
阅读更多...
Open3D 将点云投影到球面

Open3D 将点云投影到球面

目录 一、概述 二、代码实现 2.1关键函数 2.2完整代码 三、实现效果 3.1原始点云 3.2投影后点云 前期试读&#xff0c;后续会将博客加入下列链接的专栏&#xff0c;欢迎订阅 Open3D点云算法与点云深度学习案例汇总&#xff08;长期更新&#xff09;-CSDN博客 一、概述…
阅读更多...
【表单组件】地址组件新增精简模式

【表单组件】地址组件新增精简模式

07/17 主要更新模块概览 快速筛选 精简模式 触发条件 自定义域名 01 表单管理 1.1 【表单组件】-数据关联组件新增快速筛选功能 说明&#xff1a; 数据关联组件新增快速筛选功能&#xff0c;用户在数据关联组件选择数据时&#xff0c;可以通过快速筛选功能&#xff0…
阅读更多...
黑马头条Day07-app端文章搜索

黑马头条Day07-app端文章搜索

一、今日内容介绍 1. App端搜索效果图 2. 今日内容 &#xff08;1&#xff09;文章搜索 Elasticsearch环境搭建索引库创建文章搜索多条件复合查询索引数据同步 &#xff08;2&#xff09;搜索历史记录 MongoDB环境搭建异步保存搜索历史查看搜索历史列表删除搜索历史 &…
阅读更多...
Linux源码安装的Redis如何配置systemd管理并设置开机启动

Linux源码安装的Redis如何配置systemd管理并设置开机启动

文章目录 实验前提实验 实验前提 已完成源码安装并能正常启动redis /usr/local/bin/redis-server能正常启动redis 实验 vim /etc/systemd/system/redis.service内容如下&#xff1a; [unit] Descriptionredis-server Afternetwork.target[Service] Typeforking ExecStart/…
阅读更多...
从零开始创建vue3项目——包含项目初始化、element-plus、eslint、axios、router、pinia、echarts

从零开始创建vue3项目——包含项目初始化、element-plus、eslint、axios、router、pinia、echarts

项目启动 初始化vue3项目 这里建议先下载pnpm&#xff0c;下载速度更快&#xff0c;如果还没下载可以使用 npm install -g pnpm 如果遇到报错问题&#xff0c;如下 可以在命令行输入下面的指令以切换到淘宝镜像源 npm config set registry https://registry.npm.taobao.org…
阅读更多...
Facebook云手机引流运营方法

Facebook云手机引流运营方法

Facebook&#xff0c;作为全球用户数达到30亿的最大社交媒体平台&#xff0c;汇聚了各类客户群体&#xff0c;蕴藏着巨大的商业潜力。对于外贸电商而言&#xff0c;Facebook是不可或缺的营销平台。一方面&#xff0c;我们可以在Facebook上发布产品信息&#xff0c;寻找并筛选目…
阅读更多...
linux系统进程占cpu 100%解决步骤

linux系统进程占cpu 100%解决步骤

1.查找进程 ps aux 查看指定进程: ps aux | grep process_name2.根据进程查找对应的主进程 pstree -p | grep process_name 3.查看主进程目录并删除 ps -axu | grep process_name rm -rf /usr/bin/2cbbb
阅读更多...
PDF-Extract-Kit

PDF-Extract-Kit

文章目录 一、关于 PDF-Extract-Kit整体介绍效果展示 二、评测指标1、布局检测2、公式检测3、公式识别 三、安装四、模型下载1、安装 Git LFS2、从 Hugging Face 下载模型3、从 ModelScope 下载模型SDK 下载Git 下载 五、运行提取脚本六、其它待办事项协议致谢 一、关于 PDF-Ex…
阅读更多...
Spark实时（三）：Structured Streaming入门案例

Spark实时（三）：Structured Streaming入门案例

文章目录 Structured Streaming入门案例 一、Scala代码如下 二、Java 代码如下 三、以上代码注意点如下 Structured Streaming入门案例 我们使用Structured Streaming来监控socket数据统计WordCount。这里我们使用Spark版本为3.4.3版本&#xff0c;首先在Maven pom文件中导…
阅读更多...
Delphi 11.2 配置Android SDK 环境

Delphi 11.2 配置Android SDK 环境

打开 Delphi 11 点击 Tools–Options… 然后点击 Deployment–SDK Manager–Add… 这里如果配置64位就选 Android 64-bit&#xff0c;如果配置32位就选 Android 32-bit 点击 Select an SDK version–Add New… 有警告图标的就是有问题的项&#xff0c;需要手动更新一下&#xf…
阅读更多...
NO.1 Hadoop概述

NO.1 Hadoop概述

1.1 Hadoop是什么 1.2 Hadoop优势 1.3 Hadoop组成 1.3.1 HDFS架构概述 1.3.2 YARN架构概述 1.3.3 MapReduce架构概述 1.3.4 HDFS、YARN、MapReduce三者关系 1.4 大数据技术生态体系 1.5 推荐系统框架图
阅读更多...
【UE5】可反射的射线检测

【UE5】可反射的射线检测

目录 效果 步骤 一、准备射线 二、生成第一次反射后的射线 三、多次反射 四、通过循环进行多次反射 效果 步骤 一、准备射线 1. 新建一个工程&#xff0c;添加一个俯视角游戏资源包 2. 双击打开俯视角游戏地图 删除大纲中的后期处理体积使得地图可以正常显示 3. 添加一…
阅读更多...
【JavaEE初阶】线程的概念及创建

【JavaEE初阶】线程的概念及创建

目录 &#x1f4d5; 前言 &#x1f4d5; 认识线程&#xff08;Thread&#xff09; &#x1f6a9; 概念 &#x1f60a;线程是什么 &#x1f642; 为啥要有线程 &#x1f62d; 进程和线程的区别&#xff08;面试题重点&#xff09; &#x1f92d; Java的线程和操作系统线程…
阅读更多...
黑马JavaWeb企业级开发（知识清单）01——前端介绍，HTML实现标题：排版

黑马JavaWeb企业级开发（知识清单）01——前端介绍，HTML实现标题：排版

文章目录 前言一、认识web前端、HTML、CSS二、VS Code开发工具&#xff08;插件弃用问题&#xff09;三、HTML结构标签介绍1. 标签页标题< title >2. 图片标签< img >1) 常见属性2) src路径书写方式 3. 标题标签< h >4. 水平分页线标签< hr > 四、用Vs…
阅读更多...
“萝卜快跑”自动驾驶技术，夺走了谁的方向盘？

“萝卜快跑”自动驾驶技术，夺走了谁的方向盘？

在前几年&#xff0c;科幻电影中无人驾驶车自如地穿梭在城市大街小巷的场景&#xff0c;似乎还遥不可及&#xff0c;然而&#xff0c;随着“萝卜快跑”无人驾驶车辆在多个城市的成功运营&#xff0c;这一愿景已悄然变为现实。由百度Apollo倾力打造的“萝卜快跑”&#xff0c;以…
阅读更多...
基于FPGA的以太网设计（3）----详解各类xMII接口

基于FPGA的以太网设计（3）----详解各类xMII接口

1、什么是xMII接口 MII （Media Independent Interface）接口，即介质无关接口或称为媒体独立接口，它是IEEE-802.3定义的以太网行业标准。“介质无关” 表明在不对MAC硬件重新设计或替换的情况下，任何类型的PHY设备都可以正常工作。 MII接口是MAC和PHY之间的通信接口，MAC产生…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023