ROS学习之动作通信

在b站学习赵老师的ROS通信，下面给出相关学习笔记

2.4.5_动作通信_小结_哔哩哔哩_bilibili

首先，服务端的目标：

可以提取客户端请求提交的整形数据，并且累加从1到该数据之间所有整数之和

完成每累加一次都计算当前运算进度，连续返回到客户端，在最后显示求和

关于C++实现代码：

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces/action/progress.hpp"

using base_interfaces::action::Progress;
using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_2;


class ProgressActionServer: public rclcpp::Node{
public:
  ProgressActionServer():Node("progress_action_server_node_cpp"){
    RCLCPP_INFO(this->get_logger(),"Action service create");
    server_ = rclcpp_action::create_server<Progress>(
      this,
      "get_sum",
      std::bind(&ProgressActionServer::handle_goal,this,_1,_2),
      std::bind(&ProgressActionServer::handle_cancel,this,_1),
      std::bind(&ProgressActionServer::handle_accepcted,this,_1)
      );


  }
  // 处理提交目标值
  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Progress::Goal> goal){
    (void)uuid;// uuid dont used.
    if(goal->num <= 1){
      RCLCPP_INFO(this->get_logger(),"goal must biger than 1");
      return rclcpp_action::GoalResponse::REJECT;
    }
    RCLCPP_INFO(this->get_logger(),"goal rigth!");
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }
  //处理取消请求
  rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle){
    (void)goal_handle;
    RCLCPP_INFO(this->get_logger(),"receive cancel request...");
    return rclcpp_action::CancelResponse::ACCEPT;

  }
  void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle){
    int num = goal_handle->get_goal()->num;//获得目标值
    int sum = 0;
    auto feedback = std::make_shared<Progress::Feedback>();
    // sleep:
    rclcpp::Rate rate(1.0);
    auto result = std::make_shared<Progress::Result>();
    //goal_handle->publish_feedback();// 连续反馈
    for (int i =1; i<= num;i++ )
    {
      sum += i;
      double progress = i /(double)num;
      feedback->progress = progress;

      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(),"连续反馈中，进度%.2f",progress);
      // 判断是否接受到了取消请求
      //goal_handle->is_canceling()
      //goal_handle->canceled()
      if (goal_handle->is_canceling())
      {
        result->sum = sum;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(),"任务被取消");
        return;
        /* code */
      }
      
      rate.sleep();
    }
    
    
    //goal_handle->succeed();// 最终响应
    if (rclcpp::ok())
    {
      auto result = std::make_shared<Progress::Result>();
      result->sum = sum;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(),"最终相应结果%d",sum);

      /* code */
    }
    

  }
  //生成连续反馈和最终响应
  void handle_accepcted(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle){
    // 子线成处理耗时主逻辑
    std::thread(std::bind(&ProgressActionServer::execute, this,goal_handle)).detach();

  }
private:
rclcpp_action::Server<Progress>::SharedPtr server_;
  void on_timer(){

  }
  //rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  //rclcpp::TimerBase::SharedPtr timer_;
  //size_t count;
};
int main(int argc, char ** argv)
{

  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<ProgressActionServer>());

  rclcpp::shutdown();
  return 0;
}

关于Python实现代码：

import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from base_interfaces.action import Progress
import time


class ProgressActionServer(Node):
    def __init__(self):
        super().__init__("progress_action_server_node_py")
        self.get_logger().info("client create!(py)")
        self.server = ActionServer(
            self,
            Progress,
            "get_sum",
            self.execute_callback

        )
    def execute_callback(self,goal_handle):
        num = goal_handle.request.num
        sum = 0
        for i in range(1,num+1):
            sum += i
            feedback = Progress.Feedback()
            feedback.progress = i /num
            goal_handle.publish_feedback(feedback)
            self.get_logger().info("连续反馈路：%.2f"% feedback.progress)
            time.sleep(1.0)
        goal_handle.succeed()
        result = Progress.Result()
        result.sum = sum
        self.get_logger().info("计算结果:%d"% result.sum)
        return result



    def add(self,request,response):
        response.sum = request.num1 + request.num2
        self.get_logger().info("%d + %d = %d" % (request.num1,request.num2,response.sum))
        return response


def main():

    rclpy.init()
    rclpy.spin(ProgressActionServer())
    rclpy.shutdown()


if __name__ == '__main__':
    main()

对于客户端，需要实现：提交整型数据，并且处理响应结果

下面是C++实现代码：

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces/action/progress.hpp"

using base_interfaces::action::Progress;

using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_2;


class ProgressActionClient: public rclcpp::Node{
public:
  ProgressActionClient():Node("progress_action_client_node_cpp"){
    RCLCPP_INFO(this->get_logger(),"Action Client create");
    client_ = rclcpp_action::create_client<Progress>(this,"get_sum");//创建客户端

  }
  //发送目标
void send_goal(int num){
  //确保连接到服务端
  if(!client_->wait_for_action_server(10s)){
    RCLCPP_ERROR(this->get_logger(),"服务链接失败！");
    return;
  }
  // 发送请求
  auto goal = Progress::Goal();
  goal.num = num;
  rclcpp_action::Client<Progress>::SendGoalOptions options;
  options.goal_response_callback = std::bind(&ProgressActionClient::goal_response_callback,this,_1);
  options.feedback_callback = std::bind(&ProgressActionClient::feedback_callback,this,_1,_2);
  options.result_callback = std::bind(&ProgressActionClient::result_callback,this,_1);

  auto future = client_->async_send_goal(goal,options);
}

void goal_response_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle){
  if(!goal_handle){
    RCLCPP_INFO(this->get_logger(),"目标请求被拒绝");
  }
  else{
    RCLCPP_INFO(this->get_logger(),"目标处理中");
  }

}
void feedback_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle,const std::shared_ptr<const Progress::Feedback> feedback){
  (void)goal_handle;
  double progress = feedback->progress;
  int pro = (int) (progress*100);
  RCLCPP_INFO(this->get_logger(),"当前进度：%d%%",pro);
}
void result_callback(const rclcpp_action::ClientGoalHandle<Progress>::WrappedResult & result){
  //result.code
  
  if(result.code == rclcpp_action::ResultCode::SUCCEEDED)
  {
    RCLCPP_INFO(this->get_logger(),"最总结果:%d",result.result->sum);
  }
  else if (result.code == rclcpp_action::ResultCode::ABORTED)
  {
    RCLCPP_INFO(this->get_logger(),"被阻断");

    /* code */
  }
  else if (result.code == rclcpp_action::ResultCode::CANCELED)
  {
    RCLCPP_INFO(this->get_logger(),"未知异常！");
    /* code */
  }
  
  

}
private:
  rclcpp_action::Client<Progress>::SharedPtr client_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);
  if (argc != 2)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"ERROR:need two ints");
    return 1 ;
    /* code */
  }
  auto node = std::make_shared<ProgressActionClient>();
  node->send_goal(atoi(argv[1]));
  
  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

下面是Python实现代码：

import rclpy
from rclpy.node import Node
import sys
from rclpy.logging import get_logger
from rclpy.action import ActionClient
from base_interfaces.action import Progress

class ProgressActionClient(Node):
    def __init__(self):
        super().__init__("progress_action_client_node_py")
        self.get_logger().info("server create!(py)")
        self.client = ActionClient(self,Progress,"get_sum")#话题名称为get_sum 服务端和客户端要一致

    def send_goal(self,num):
        # 连接服务端
        self.client.wait_for_server()
        # 发送请求
        goal = Progress.Goal()
        goal.num = num
        self.future = self.client.send_goal_async(goal,self.fb_callback)
        self.future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self,future):
        # 获取目标句并
        goal_handle = future.result()

        # 判断目标是否正常处理
        if not goal_handle.accepted:
            self.get_logger().error("目标被拒绝")
            return
        self.get_logger().info("目标被接受，正在处理中")
        self.result_future = goal_handle.get_result_async()
        self.result_future.add_done_callback(self.get_result_callback)

    # 回调函数：
    def fb_callback(self,fb_msg):
        progress = fb_msg.feedback.progress
        self.get_logger().info("连续反馈数据：%.2f"% progress)
    
    # 目标最终响应结果

    # 
    def get_result_callback(self,future):
        result = future.result().result
        self.get_logger().info("最终结果：%d"%result.sum)


def main():

    rclpy.init()
    if len(sys.argv) != 2:
        get_logger("rclpy").error("清提交一个整形数据")

        return
    action_client = ProgressActionClient()
    action_client.send_goal(int(sys.argv[1]))
    rclpy.spin(action_client)
    rclpy.shutdown()


if __name__ == '__main__':
    main()

给出C++实现中，需要修改的Cmakelist文件:

cmake_minimum_required(VERSION 3.8)
project(cpp03_action)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(base_interfaces REQUIRED)

add_executable(demo01_action_server src/demo01_action_server.cpp)
add_executable(demo02_action_client src/demo02_action_client.cpp)

target_include_directories(demo01_action_server PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(demo01_action_server PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  demo01_action_server
  "rclcpp"
  "rclcpp_action"
  "base_interfaces"
)

ament_target_dependencies(
  demo02_action_client
  "rclcpp"
  "rclcpp_action"
  "base_interfaces"
)

install(TARGETS demo01_action_server
  DESTINATION lib/${PROJECT_NAME})

install(TARGETS demo02_action_client
  DESTINATION lib/${PROJECT_NAME})


if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

Python的setup.py文件：

from setuptools import find_packages, setup

package_name = 'py03_action'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='xwh',
    maintainer_email='xwh@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'demo01_action_server_py = py03_action.demo01_action_server_py:main',
            'demo02_action_client_py = py03_action.demo02_action_client_py:main'
        ],
    },
)

先：colcon build --packages-select cpp03_action

. install/setup.bash

分别打开两个终端：

1、ros2 run cpp03_action demo01_action_server

2、ros2 run cpp03_action demo02_action_client 10

 给出C++下实现的结果：

先：colcon build --packages-select cpp03_action

. install/setup.bash

分别打开两个终端：

1、ros2 run py03_action demo01_action_server_py

2、ros2 run py03_action demo02_action_client_py 15

给出Python环境下的结果：

关于博主在学习时候，遇到的坑

在修改py的package.xml中 注意标点符号的使用 
            'demo01_talker_str_py = py01_topic.demo01_talker_str_py:main',
            'demo02_listener_str_py = py01_topic.demo02_listener_str_py:main',
            'demo03_talker_stu_py = py01_topic.demo03_talker_stu_py:main',
            'demo04_listener_stu_py = py01_topic.demo04_listener_stu_py:main'
            如果没有后面的逗号 是无法执行ros2 run---

在写完好service.cpp文件后 利用终端进行发送订阅的数据
ros2 service call /add_ints base_interfaces/srv/Addints "{'num1':10,'num2':20}"

在action中的 客户端在终端中 输入的指令为：
ros2 action send_goal /get_sum base_interfaces/action/Progress -f "{'num':10}"

上面仅仅只是代码，如果只看代码是无法完全了解的，建议跟着赵老师视频学习，并且自己手敲几遍，熟悉熟悉。

所谓，键盘敲烂，年薪千万。（改用赵老师的话送给大家，希望看到这里的各位将来都能找到心满意足的好工作！）