ROS2 学习笔记2 通信机制核心

I'm coming!!

于 2025-04-20 21:40:47 发布

阅读量635

点赞数 23

CC 4.0 BY-SA版权

文章标签：学习笔记

本文链接：https://blog.youkuaiyun.com/tech1001/article/details/147015143

准备工作：创建接口文件功能包

ros2 pkg create --build-type ament_cmake base_interfaces_demo

接口功能包只能用 C++
创建功能包时可以在依赖中加入 base_interfaces_demo

1.话题通信

1.1 原生消息发布（框架搭建）

#include "rclcpp/rclcpp.hpp"

class Talker: public rclcpp::Node{
  public:
    Talker():Node("talker_node_cpp"){
      RCLCPP_INFO(this->get_logger(),"发布节点创建!");
    }
};

int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<Talker>());
  rclcpp::shutdown();
  return 0;
}

class Talker: public rclcpp::Node

定义了一个类 Talker ，继承自 rclcpp::Node

Talker():Node("talker_node_cpp")

Talker()：这是 Talker 类的构造函数。
Node(“talker_node_cpp”)：调用了基类 rclcpp::Node 的构造函数，并传递了一个字符串参数 “talker_node_cpp”，这是节点的名称。在 ROS 2 中，每个节点都需要有一个唯一的名称，用于标识它在 ROS 系统中的身份。

rclcpp::spin(std::make_shared<Talker>())

std::make_shared()
std::make_shared 是 C++11 引入的一个函数，用于创建一个 std::shared_ptr 智能指针。
Talker 是之前定义的节点类。
这段代码的作用是创建一个 Talker 类型的共享指针对象。
rclcpp::spin()
rclcpp::spin() 是 ROS 2 提供的一个函数，用于启动节点的事件循环。
事件循环是 ROS 2 节点的核心机制，它负责处理节点的回调函数（如订阅消息的回调、定时器回调等）。
rclcpp::spin() 会阻塞当前线程，直到节点被销毁或程序结束。

1.2 发布方逻辑实现

C++

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class Talker: public rclcpp::Node{
  public:
    Talker():Node("talker_node_cpp"),count(0){
      RCLCPP_INFO(this->get_logger(),"发布节点创建!");
      // 3-1.创建消息发布方
      /*
        模板：被发布的消息类型；
        参数：
          1.话题名称；
          2.QOS(消息队列长度)。
        返回值：发布对象指针。
      */
      publisher_ = this->create_publisher<std_msgs::msg::String>("chatter",10);
      // 3-2.创建定时器；
      /*
        参数：
          1.时间间隔；
          2.回调函数；
        返回值：定时器对象指针。
      */
      timer_ = this->create_wall_timer(1s,std::bind(&Talker::on_timer,this));
    }
  private:
    void on_timer(){
      // 3-3.组织并发布消息。
      auto message = std_msgs::msg::String();
      message.data = "hello_world!" + std::to_string(count++);
      RCLCPP_INFO(this->get_logger(),"发布方发布的消息：%s",message.data.c_str());
      publisher_ ->publish(message);
    }
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    size_t count;
};

int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<Talker>());
  rclcpp::shutdown();
  return 0;
}

using namespace std::chrono_literals;
该命名空间下，定时器可以直接传入所需的时间间隔(timer_ = this->create_wall_timer(1s,std::bind(&Talker::on_timer,this));)

timer_ = this->create_wall_timer(1s,std::bind(&Talker::on_timer,this))
作用: 创建一个周期为1秒的定时器，触发on_timer回调。
参数解析:
参数1: 1s 表示时间间隔（需要using namespace std::chrono_literals;）。
参数2: std::bind(&Talker::on_timer, this) 将成员函数on_timer绑定到当前对象(this)。

Talker():Node("talker_node_cpp"),count(0)
初始化 count = 0

Python

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class Talker(Node):
    def __init__(self):
        super().__init__("talker_node_py")
        self.get_logger().info("发布方创建")
        # 设置计数器
        self.count = 0
        self.publisher = self.create_publisher(String,"chatter",10)
        self.timer = self.create_timer(1.0,self.on_timer)

    def on_timer(self):
        message = String()
        message.data = "hello world(python)!" + str(self.count)
        self.publisher.publish(message)
        self.count += 1
        self.get_logger().info("发布的数据:%s" % message.data)

def main():
    rclpy.init()
    rclpy.spin(Talker())
    rclpy.shutdown()

if __name__ == '__main__':
    main()

1.3 订阅方逻辑实现

C++

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

class Listener: public rclcpp::Node{
public:
    Listener():Node("listener_node_cpp"){
        RCLCPP_INFO(this->get_logger(),"订阅方创建！");
        // 创建订阅方；
        /*
            模板：消息类型；
            参数：
                话题名称；
                QOS，队列长度；
                回调函数。
            返回值：订阅对象指针。
        */
        Subscription_ = this->create_subscription<std_msgs::msg::String>("chatter",10,std::bind(&Listener::do_cb,this,std::placeholders::_1));
    }
private:
    void do_cb(const std_msgs::msg::String &msg){
        // 解析并输出数据。
        RCLCPP_INFO(this->get_logger(),"订阅到的消息是：%s",msg.data.c_str());
    }
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr Subscription_;
};

int main(int argc, char const *argv[])
{
    rclcpp::init(argc,argv);
    rclcpp::spin(std::make_shared<Listener>());
    rclcpp::shutdown();
    return 0;
}

std::bind(&Listener::do_cb,this,std::placeholders::_1)
占位符含义
std::placeholders::_1 表示生成的绑定函数对象在被调用时，其第一个参数将传递给原成员函数 Listener::do_cb 的第一个形参。占位符的编号（如 _1, _2）对应调用时参数的位置。
参数传递逻辑
成员函数 Listener::do_cb 隐式接收 this 指针作为第一个参数（通过 std::bind 的第二个参数显式绑定为当前对象的 this）。
若 do_cb 的声明为 void Listener::do_cb(int arg)，则绑定后生成的函数对象调用 func(42) 时，实际执行 this->do_cb(42)。
参数匹配规则
占位符数量和顺序需与成员函数参数一致。例如，若 do_cb 有两个参数，则需要用 _1 和 _2（或绑定部分值）。
类型必须兼容，否则导致编译错误或隐式转换。
总结
std::placeholders::_1 在此表示：调用绑定后的函数对象时，其第一个实参将传递给 Listener::do_cb 的第一个形参，而 this 确保在正确的对象实例上调用该成员函数

Python

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class Listener(Node):
    def __init__(self):
        super().__init__("listener_node_py")
        self.get_logger().info("订阅方")
        self.subscription = self.create_subscription(String,"chatter",self.do_cb,10)

    def do_cb(self,msg):
        self.get_logger().info("订阅到的数据：%s" % msg.data)

def main():
    rclpy.init()
    rclpy.spin(Listener())
    rclpy.shutdown()

if __name__=='__main__':
    main()

self.subscription注意不要写成 subscriptions

do_cb(self,msg)msg 表示订阅到的消息

配置文件(setup.py)

    entry_points={
        'console_scripts': [
            'demo01_talker_str_py = py01_topic.demo01_talker_str_py:main',
            'demo02_listener_str_py = py01_topic.demo02_listener_str_py:main'
        ],

代码片段配置

点击 VSCode 左下角 -> 代码片段
在这里插入图片描述
里面有模板如下

	// Example:
	// "Print to console": {
	// 	"prefix": "log",
	// 	"body": [
	// 		"console.log('$1');",
	// 		"$2"
	// 	],
	// 	"description": "Log output to console"
	// }

配置如下

		"Print to console": {
		"prefix": "ros2_node_cpp",
		"body": [
			"#include \"rclcpp/rclcpp.hpp\"",
			"    ",
			"class MyNode: public rclcpp::Node{",
			"public:",
			"  MyNode():Node(\"mynode_node_cpp\"){",
			"    ",
			"  }",
			"};",
			"    ",
			"int main(int argc, char const *argv[])",
			"{",
			"  rclcpp::init(argc,argv);",
			"  rclcpp::spin(std::make_shared<MyNode>());",
			"  rclcpp::shutdown();",
			"  return 0;",
			"}"
			
		],
		"description": "Log output to console"
	}
	}

每一行要用双引号引起来，由逗号隔开，且中间不能有Tab，要换成空格

1.4 自定义消息类型

base_interfaces_demo 下新建 msg 和消息文件
在这里插入图片描述

string name
int32  age
float64 height

package.xml 配置

  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

分别为编译时依赖，运行时依赖，所属的功能包
Tips: 如果记不住可以通过指令ros2 pkg list | grep -i rosidl查询

ros2 pkg list | grep -i rosidl
rosidl_adapter
rosidl_cli
rosidl_cmake
rosidl_default_generators
rosidl_default_runtime
rosidl_generator_c
rosidl_generator_cpp
rosidl_generator_py
rosidl_parser
rosidl_runtime_c
rosidl_runtime_cpp
rosidl_runtime_py
rosidl_typesupport_c
rosidl_typesupport_cpp
rosidl_typesupport_fastrtps_c
rosidl_typesupport_fastrtps_cpp
rosidl_typesupport_interface
rosidl_typesupport_introspection_c
rosidl_typesupport_introspection_cpp

这两行会报错，删除就行

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

最终效果：

  <name>base_interfaces_demo</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="herrscher@todo.todo">herrscher</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

CMakeLists 配置

加入该部分

find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces( ${PROJECT_NAME}
  "msg/Student.msg"
)

括号内为 msg 的路径

编译colcon build --packages-select base_interfaces_demo

可以通过命令检查

ros2 interface show base_interfaces_demo/msg/Student 
string name
int32  age
float64 height

1.5 自定义接口消息

C++

生成的接口文件可能存在路径配置问题导致报错和无法自动补全
需要在C++的路径配置里添加${workspaceFolder}/install/base_interfaces_demo/include/**
在这里插入图片描述

发布方实现

#include "base_interfaces_demo/msg/student.hpp"
#include "rclcpp/rclcpp.hpp"

using base_interfaces_demo::msg::Student;
using namespace std::chrono_literals;

class TalkerStu: public rclcpp::Node{
public:
    TalkerStu():Node("talkerstu_node_cpp"){
            publisher_ = this->create_publisher<Student>("chatter_stu",10);
            time_ = this->create_wall_timer(500ms,std::bind(&TalkerStu::on_timer,this));
    }
private:
    void on_timer(){
        auto stu = Student();
        stu.name = "DLS";
        stu.age = 8;
        stu.height = 1.40;

        publisher_->publish(stu);
        RCLCPP_INFO(this->get_logger(),"发布的消息:(%s,%d,%.2f)",stu.name.c_str(),stu.age,stu.height);
    }
    rclcpp::Publisher<Student>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr time_;
};

int main(int argc, char const *argv[])
{
    rclcpp::init(argc,argv);
    rclcpp::spin(std::make_shared<TalkerStu>());
    rclcpp::shutdown();
    return 0;
}

using 关键字
将特定名称引入当前作用域，避免重复书写冗长的命名空间。

订阅方实现

#include "base_interfaces_demo/msg/student.hpp"
#include "rclcpp/rclcpp.hpp"

using base_interfaces_demo::msg::Student;

class ListenerStu: public rclcpp::Node{
public:
    ListenerStu():Node("listenerstu_node_cpp"){
        subscription_ = this->create_subscription<Student>("chatter_stu",10,std::bind(&ListenerStu::do_cb,this,std::placeholders::_1));

    }
private:
    void do_cb(const Student &stu){
        RCLCPP_INFO(this->get_logger(),"订阅到的数据:(%s,%d,%.2f)",stu.name.c_str(),stu.age,stu.height);
    }
    rclcpp::Subscription<Student>::SharedPtr subscription_;
};

int main(int argc, char const *argv[])
{
    rclcpp::init(argc,argv);
    rclcpp::spin(std::make_shared<ListenerStu>());
    rclcpp::shutdown();
    return 0;
}

Python

和 C++ 一样需要经过配置才能正常使用
Ctrl+Shift+P打开，找到该选项
在这里插入图片描述
修改如下

{
    "python.autoComplete.extraPaths": [
        "/home/herrscher/ws01_plumbing/install/base_interfaces_demo/local/lib/python3.10/dist-packages"
    ],
    "python.analysis.extraPaths": [
        "/home/herrscher/ws01_plumbing/install/base_interfaces_demo/local/lib/python3.10/dist-packages" 
    ]
}

发布方实现

import rclpy
from rclpy.node import Node
from base_interfaces_demo.msg import Student

class TalkerStu(Node):
    def __init__(self):
        super().__init__("talkerstu_node_py")
        self.publisher = self.create_publisher(Student,"chatter_str",10)
        self.timer = self.create_timer(0.5,self.on_timer)
    
    def on_timer(self):
        stu = Student()
        stu.name = "HKS"
        stu.age = 6
        stu.height = 1.40
        self.publisher.publish(stu)

def main():
    rclpy.init()
    rclpy.spin(TalkerStu())
    rclpy.shutdown()

if __name__ == '__main__':
    main()

订阅方实现

import rclpy
from rclpy.node import Node
from base_interfaces_demo.msg import Student

class ListenerStu(Node):
    def __init__(self):
        super().__init__("listenerstu_node_py")
        self.subscription = self.create_subscription(Student,"chatter_stu",self.do_cb,10)

    def do_cb(self,stu):
        self.get_logger().info("%s,%d,%.2f" % (stu.name,stu.age,stu.height))

def main():
    rclpy.init()
    rclpy.spin(ListenerStu())
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2.0 服务通信

2.1 自定义接口文件

在 base_interfaces_demo 下新建 srv 目录

int32 num1
int32 num2
---
int32 sum

配置方法和检查方法同 msg

2.2 框架搭建

C++

服务端

#include "rclcpp/rclcpp.hpp"

class AddIntsService: public rclcpp::Node{
public:
  AddIntsService():Node("addints_service_node_cpp"){
      RCLCPP_INFO(this->get_logger(),"服务端启动");
  }
};

int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<AddIntsService>());
  rclcpp::shutdown();
  return 0;
}

客户端


class AddIntsClient: public rclcpp::Node{
public:
  AddIntsClient():Node("addints_client_node_cpp"){
    RCLCPP_INFO(get_logger(),"客户端启动");
  }
};
    
int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  auto client = std::make_shared<AddIntsClient>();
  rclcpp::shutdown();
  return 0;
}

Python

服务端

import rclpy
from rclpy.node import Node

class AddIntsSerer(Node):
    def __init__(self):
        super().__init__("addints_serer_node_py")
        self.get_logger().info("服务端创建")

def main():
    rclpy.init()
    rclpy.spin(AddIntsSerer())
    rclpy.shutdown()

if __name__ == '__main__':
    main()

服务端

import rclpy
from rclpy.node import Node
    
class AddIntsClient(Node):
    def __init__(self):
        super().__init__("addints_client_node_py")
        self.get_logger().info("客户端启动")
    
def main():
    rclpy.init()
    client = AddIntsClient()
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()

2.3 服务端实现

C++

#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/srv/add_ints.hpp"

using base_interfaces_demo::srv::AddInts;
using std::placeholders::_1;
using std::placeholders::_2;
    
class AddIntsClient: public rclcpp::Node{
public:
  AddIntsClient():Node("addints_server_node_cpp"){
    RCLCPP_INFO(get_logger(),"服务端启动");
    server_ = this->create_service<AddInts>("add_ints",std::bind(&AddIntsClient::add,this,_1,_2));
  }
private:
    void add(const AddInts::Request::SharedPtr req, const AddInts::Response::SharedPtr res){
        res->sum = req->num1 + req->num2;
        RCLCPP_INFO(this->get_logger(),"%d + %d = %d",req->num1,req->num2,res->sum);
    }
  rclcpp::Service<AddInts>::SharedPtr server_;
};
    
int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  auto client = std::make_shared<AddIntsClient>();
  rclcpp::shutdown();
  return 0;
}

Python

import rclpy
from rclpy.node import Node
from base_interfaces_demo.srv import AddInts

class AddIntsSerer(Node):
    def __init__(self):
        super().__init__("addints_serer_node_py")
        self.get_logger().info("服务端创建")
        self.server = self.create_service(AddInts,"add_ints",self.add)

    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(AddIntsSerer())
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2.4 客户端实现

C++

#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/srv/add_ints.hpp"

using base_interfaces_demo::srv::AddInts;
using namespace std::chrono_literals;

class AddIntsClient: public rclcpp::Node{
public:
  AddIntsClient():Node("addints_client_node_cpp"){
      RCLCPP_INFO(this->get_logger(),"客户端启动");
      client_ = this->create_client<AddInts>("add_ints");
  }
  bool connect_server(){
    while (!client_->wait_for_service(2s))
    {
      // 对 ctrl+c 这个操作作出特殊操作
      // 判断 ctrl+c 按下
      if(!rclcpp::ok())
      {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"强行终止客户端!");
        return false;
      }

      RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务器连接中");
    }
    
    return true;
  }
  // 请求发送
  rclcpp::Client<AddInts>::FutureAndRequestId send_request(int num1,int num2){
    auto request = std::make_shared<AddInts::Request>();
    request->num1 = num1;
    request->num2 = num2;
    return client_->async_send_request(request);
  }
private:
  rclcpp::Client<AddInts>::SharedPtr client_;  
};

int main(int argc, char const *argv[])
{
  if(argc != 3)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"请提交两个整数！");
    return 1;
  }
  rclcpp::init(argc,argv);
  auto client = std::make_shared<AddIntsClient>();
  bool flag = client->connect_server();
  if(!flag)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"服务器连接失败，程序退出");
    return 0;
  }
  auto future = client->send_request(atoi(argv[1]),atoi(argv[2]));
  if(rclcpp::spin_until_future_complete(client,future) == rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(client->get_logger(),"响应成功,sum=%d",future.get()->sum);
  }
  else
  {
    RCLCPP_INFO(client->get_logger(),"响应失败");
  }
  rclcpp::shutdown();
  return 0;
}

Python

import rclpy
from rclpy.node import Node
from rclpy.logging import get_logger
import sys
from base_interfaces_demo.srv import AddInts
    
class AddIntsClient(Node):
    def __init__(self):
        super().__init__("addints_client_node_py")
        self.get_logger().info("客户端启动")
        self.client = self.create_client(AddInts,"add_ints")
        while not self.client.wait_for_service(1.0):
            self.get_logger().info("服务连接中!")

    def send_request(self):
        request = AddInts.Request()
        request.num1 = int(sys.argv[1])
        request.num2 = int(sys.argv[2])
        self.future = self.client.call_async(request)
    
def main():
    if len(sys.argv) != 3:
        get_logger("rclpy").info("请提交两个整形数据！")
        return
    
    rclpy.init()
    client = AddIntsClient()
    client.send_request()
    rclpy.spin_until_future_complete(client,client.future)
    try:
        response = client.future.result()
        client.get_logger().info("响应结果:sum = %d" % response.sum)
    except Exception:
        client.get_logger().error("服务响应失败！")
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()

3.0 动作通信

3.1 自定义接口文件

C++依赖
rclcpp rclcpp_action base_interfaces_demo

Python依赖
rclpy base_interfaces_demo

int32 num
---
int32 sum
---
float64 progress

package.xml配置
依赖于这几行

<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
<member_of_group>rosidl_interface_packages</member_of_group>

CmakeLists.txt配置和话题/服务通信相同

3.2 服务端实现

C++

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

using base_interfaces_demo::action::Progress;
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(),"服务端创建");
    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_accepted,this,_1)
    );
  }
  // 1.处理提交的目标值（回调函数）
  // std::function<GoalResponse(const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>;
  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const Progress::Goal> goal){
    (void)uuid;
    // 业务逻辑：判断提交的数据是否大于1,否则拒绝
    if(goal->num <= 1){
      RCLCPP_INFO(this->get_logger(),"提交的目标值必须大于1");
      return rclcpp_action::GoalResponse::REJECT;
    }
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }
  //2. 处理取消请求
  // std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
  rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle){
    (void)goal_handle;
    RCLCPP_INFO(this->get_logger(),"任务请求取消");
    return rclcpp_action::CancelResponse::ACCEPT;
  }
  //3. 生成连续反馈和最终响应（回调函数）
  void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle){
    // 1. 生成连续反馈给客户端
    //void publish_feedback(std::shared_ptr<base_interfaces_demo::action::Progress_Feedback> feedback_msg)
    // goal_handle->publish_feedback()
    int num = goal_handle->get_goal()->num;
    int sum = 0;
    auto feedback = std::make_shared<Progress::Feedback>();
    auto result = std::make_shared<Progress::Result>();
    rclcpp::Rate rate(1.0);
    for (int i = 0; i <= num; i++)
    {
      sum += i;
      double progress = i / (double)num;
      feedback->progress = progress;
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(),"feedback:%.2f",progress);
      if(goal_handle->is_canceling())
      {
        result->sum = sum;
        goal_handle->canceled(result);
        return;
      }
      rate.sleep();
    }

    // 2.生成最终响应结果
    //void succeed(std::shared_ptr<base_interfaces_demo::action::Progress_Result> result_msg)
    // goal_handle->succeed()
    if(rclcpp::ok())
    {
      result->sum = sum;
      goal_handle->succeed(result);
    }
  }
  // std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;
  void handle_accepted(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_;
};
    
int main(int argc, char const *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_demo.action import Progress
import time
    
class ProgressActionServer(Node):
    def __init__(self):
        super().__init__("progress_action_server_node_py")
        self.get_logger().info("服务端创建")
        self.server = ActionServer(
            self,
            Progress,
            "get_sum",
            self.execute_callback
        )

    def execute_callback(self,goal_handle):
        # 处理提交的目标值（回调函数）-- 默认实现
        # 处理取消请求 -- 默认实现
        # 1.生成连续反馈
        num = goal_handle.request.num
        sum = 0
        for i in range(1,num + 1):
            sum += 1
            feedback = Progress.Feedback()
            feedback.progress = 1 / num
            goal_handle.publish_feedback(feedback)
            self.get_logger().info("连续反馈:%.2f" % feedback.progress)
            time.sleep(1.0)
        # 2. 响应最终结果
        goal_handle.succeed()
        result = Progress.Result()
        result.sum = sum
        self.get_logger().info("sum = %d" % result.sum)
        return result

    
def main():
    rclpy.init()
    rclpy.spin(ProgressActionServer())
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()

3.3 客户端实现

C++

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

using base_interfaces_demo::action::Progress;
using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_2;
    
class ProgressActionClienr: public rclcpp::Node{
public:
  ProgressActionClienr():Node("progress_action_clienr_node_cpp"){
    RCLCPP_INFO(this->get_logger(),"客户端创建");
    /*
    rclcpp_action::Client<ActionT>::SharedPtr 
    create_client<ActionT, NodeT>(NodeT node, 
    const std::string &name, 
    rclcpp::CallbackGroup::SharedPtr group = nullptr, 
    const rcl_action_client_options_t &options = rcl_action_client_get_default_options())
    */
   client_ = rclcpp_action::create_client<Progress>(this,"get_sum");
  }
    // 1. 发送请求；
    void send_goal(int num){
      // 1.1 连接服务器
      if(!client_->wait_for_action_server(10s)){
        RCLCPP_ERROR(this->get_logger(),"连接失败");
        return;
      }
      // 2. 发送请求
      /*
      std::shared_future<rclcpp_action::ClientGoalHandle<base_interfaces_demo::action::Progress>::SharedPtr> 
      async_send_goal(
        const base_interfaces_demo::action::Progress::Goal &goal, 
        const rclcpp_action::Client<base_interfaces_demo::action::Progress>::SendGoalOptions &options)
      */
      auto goal = Progress::Goal();
      goal.num = num;
      rclcpp_action::Client<Progress>::SendGoalOptions options;
      options.goal_response_callback = std::bind(&ProgressActionClienr::goal_response_callback,this,_1);
      options.feedback_callback = std::bind(&ProgressActionClienr::feedback_callback,this,_1,_2);
      options.result_callback = std::bind(&ProgressActionClienr::result_feedback,this,_1);
      auto future = client_ -> async_send_goal(goal,options);
    }
    // 2. 处理关于目标值的服务端响应(回调函数)；
    /*
    using GoalHandle = ClientGoalHandle<ActionT>;
    using GoalResponseCallback = std::function<void (typename GoalHandle::SharedPtr)>;
    */
    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(),"目标处理中！");
      }
    }
    // 3. 处理连续反馈(回调函数)；
    /*
    using FeedbackCallback =
    std::function<void (
      typename ClientGoalHandle<ActionT>::SharedPtr,
      const std::shared_ptr<const Feedback>)>;
    */
    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);
    }
    // 4. 处理最终响应(回调函数)。
    //using ResultCallback = std::function<void (const WrappedResult & result)>
    void result_feedback(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(),"被中断");
        } else
        if(result.code == rclcpp_action::ResultCode::CANCELED)
        {
          RCLCPP_INFO(this->get_logger(),"被取消");
        }
        else{
          RCLCPP_INFO(this->get_logger(),"未知状态");
        }
    }
private:
  rclcpp_action::Client<Progress>::SharedPtr client_;
};
    
int main(int argc, char const *argv[])
{
  if(argc != 2){
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"请提交一个整形数据");
    return 1;
  }
  rclcpp::init(argc,argv);
  auto node = std::make_shared<ProgressActionClienr>();
  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_demo.action import Progress
    
class ProgressActionClient(Node):
    def __init__(self):
        super().__init__("progress_action_client_node_py")
        self.get_logger().info("客户端创建")
        # node, action_type, action_name
        self.client = ActionClient(self,Progress,"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_request_callback)

    def goal_request_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 get_result_callback(self,future):
        result = future.result().result
        self.get_logger().info("最终结果:%d" % result.sum)

    def fb_callback(self,fb_msg):
        progress = fb_msg.feedback.progress
        self.get_logger().info("连续反馈数据:%.2f" % progress)

def main():
    # 动态解析传入的参数
    if len(sys.argv) != 2:
        get_logger("rclpy").error("清提交一个整形数据")
        return
    
    rclpy.init()
    action_client = ProgressActionClient()
    action_client.send_goal(int(sys.argv[1]))
    rclpy.spin(action_client)
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()

Python没有补全的问题
在这里插入图片描述

4.0 参数服务器

C++依赖:rclcpp
Python依赖:rclpy

4.1 参数简介

C++ API

#include "rclcpp/rclcpp.hpp"
    
class MyParam: public rclcpp::Node{
public:
  MyParam():Node("my_param_node_cpp"){
    RCLCPP_INFO(this->get_logger(),"参数API使用");
    // 3-1.参数对象创建；
    rclcpp::Parameter p1("car_name","tiger");
    rclcpp::Parameter p2("height",1.68);
    rclcpp::Parameter p3("wheels",4);
    // 3-2.参数对象解析
    RCLCPP_INFO(this->get_logger(),"car_name = %s",p1.as_string().c_str());
    RCLCPP_INFO(this->get_logger(),"height = %.2f",p2.as_double());
    RCLCPP_INFO(this->get_logger(),"wheels = %ld",p3.as_int());

    // 获取参数键
    // 键名
    RCLCPP_INFO(this->get_logger(),"name = %s",p1.get_name().c_str());
    // 键类型
    RCLCPP_INFO(this->get_logger(),"type = %s",p1.get_type_name().c_str());
    // 键值(转化为字符串)
    RCLCPP_INFO(this->get_logger(),"value2string = %s",p2.value_to_string().c_str());
  }
};
    
int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<MyParam>());
  rclcpp::shutdown();
  return 0;
}

Python API

import rclpy
from rclpy.node import Node
import rclpy.parameter
    
class MyParam(Node):
    def __init__(self):
        super().__init__("my_param_node_py")
        self.get_logger().info("参数API使用")
        p1 = rclpy.Parameter("car_name",value = "Tiger")
        p2 = rclpy.Parameter("widht",value = 1.5)
        p3 = rclpy.Parameter("wheels",value = 2)

        self.get_logger().info("car_name = %s" % p1.value)
        self.get_logger().info("widht = %s" % p2.value)
        self.get_logger().info("wheels = %s" % p3.value)

        self.get_logger().info("key = %s" % p1.name)
    
def main():
    rclpy.init()
    rclpy.spin(MyParam())
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()

4.2 服务端实现

C++

#include "rclcpp/rclcpp.hpp"
    
class ParamServer: public rclcpp::Node{
public:
  ParamServer():Node("param_server_node_cpp",rclcpp::NodeOptions().allow_undeclared_parameters(true)){
    RCLCPP_INFO(this->get_logger(),"参数服务器启动");
    
  }
  void declare_param(){
    RCLCPP_INFO(this->get_logger(),"-----------增-----------");
    this->declare_parameter("car_name","tiger");
    this->declare_parameter("width",1.55);
    this->declare_parameter("wheels",5);
    // 也可以用于设置新参数,但是要求增删权限
    this->set_parameter(rclcpp::Parameter("height",2.00));
  }

  void get_param(){
    RCLCPP_INFO(this->get_logger(),"-----------查-----------");
    // this->get_parameter()
    // this->get_parameters()
    // this->has_parameter()
    //获取指定参数
    auto car = this->get_parameter("car_name");
    RCLCPP_INFO(this->get_logger(),"key = %s, value = %s",car.get_name().c_str(),car.as_string().c_str());
    // 获取一些参数
    auto params = this->get_parameters({"car_name","width","wheels"});
    for(auto &&param : params)
    {
        RCLCPP_INFO(this->get_logger(),"(%s = %s)",param.get_name().c_str(),param.value_to_string().c_str());
    }
    // 查看是否包含
    RCLCPP_INFO(this->get_logger(),"car_name? %d",this->has_parameter("car_name"));
    RCLCPP_INFO(this->get_logger(),"heigth? %d",this->has_parameter("heigth"));
}

  void update_param(){
    RCLCPP_INFO(this->get_logger(),"-----------改-----------");
    this->set_parameter(rclcpp::Parameter("width",1.75));
    RCLCPP_INFO(this->get_logger(),"width = %.2f",this->get_parameter("width").as_double());
  }

  void del_param(){
    RCLCPP_INFO(this->get_logger(),"-----------删-----------");
    //this->undeclare_parameter("car_name"); //不能删除声明的参数
    this->undeclare_parameter("height");
    RCLCPP_INFO(this->get_logger(),"heigth %d",this->has_parameter("height"));
  }
};
    
int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  auto node = std::make_shared<ParamServer>();

  node->declare_param();
  node->get_param();
  node->update_param();
  node->del_param();

  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

Python

4.3 客户端实现

C++

#include "rclcpp/rclcpp.hpp"
    
using namespace std::chrono_literals;

class ParamClient: public rclcpp::Node{
public:
  ParamClient():Node("param_client_node_cpp"){
    RCLCPP_INFO(this->get_logger(),"参数客户端启动");

    // 创建参数客户端对象
    // 参数1:当前对象依赖的节点
    // 参数2:参数服务器节点名称
    param_client_ = std::make_shared<rclcpp::SyncParametersClient>(this,"param_server_node_cpp");
    /*
        问:服务通信通过话题关联,为什么参数客户端要通过节点?
        答:
            1.参数服务端启动后,底层封装了多个服务通信服务端.
            2.每个服务端话题都采用了 /节点/xxxx 格式.
            3.参数客户端创建后,也会封装多个服务通信的客户端.
            4.这些客户端与服务端呼应,要使用相同的话题,因此客户端创建需要使用服务端节点名称.
    */
  }
  bool connect_server(){
    while (!param_client_->wait_for_service(1s))
    {
        if(!rclcpp::ok())
        {
            return false;
        }
        RCLCPP_INFO(this->get_logger(),"服务连接中");
    }

    return true;
  }
  // 查询参数
  void get_param(){
    RCLCPP_INFO(this->get_logger(),"-----参数查询操作-----");
        // 获取单个参数
        std::string car_name = param_client_->get_parameter<std::string>("car_name");
        double width = param_client_->get_parameter<double>("width");
        RCLCPP_INFO(this->get_logger(),"car_name = %s",car_name.c_str());
        RCLCPP_INFO(this->get_logger(),"width = %.2f",width);
        // 获取多个参数
        auto params = param_client_->get_parameters({"car_name","width","wheels"});
        for (auto &&param : params)
        {
          RCLCPP_INFO(this->get_logger(),"%s = %s",param.get_name().c_str(),param.value_to_string().c_str());
        }
    
        // 判断是否包含某个参数
        RCLCPP_INFO(this->get_logger(),"car_name? %d",param_client_->has_parameter("car_name"));
        RCLCPP_INFO(this->get_logger(),"height? %d",param_client_->has_parameter("height"));    
  }
  // 修改参数
  void update_param(){
    RCLCPP_INFO(this->get_logger(),"-----参数修改操作-----");
    param_client_->set_parameters({rclcpp::Parameter("car_name","pig"),
                                   rclcpp::Parameter("width",3.0),
                                   rclcpp::Parameter("length",5.0)
                                  });
    RCLCPP_INFO(this->get_logger(),"length:%.2f",param_client_->get_parameter<double>("length"));
  }
private:
  rclcpp::SyncParametersClient::SharedPtr param_client_;
};
    
int main(int argc, char const *argv[])
{
  rclcpp::init(argc,argv);
  auto client = std::make_shared<ParamClient>();
  bool flag = client->connect_server();
  if(!flag){
    return 0;
  }
  client->get_param();
  client->update_param();
  client->get_param();

  rclcpp::shutdown();
  return 0;
}

Python

import rclpy
from rclpy.node import Node
    
class ParamServer(Node):
    def __init__(self):
        # 允许删除参数需要提前声明
        super().__init__("param_server_node_py",allow_undeclared_parameters=True)
        self.get_logger().info("参数服务器")
    
    def declare_param(self):
        self.get_logger().info("-------增-------")
        self.declare_parameter("car_name","tiger")
        self.declare_parameter("width",1.55)
        self.declare_parameter("wheels",5)

        self.set_parameters([rclpy.Parameter("haha",value="xixi")])
    
    def get_param(self):
        self.get_logger().info("-------查-------")
        # 获取制定参数
        car_name = self.get_parameter("car_name")
        self.get_logger().info("%s = %s" % (car_name.name, car_name.value))
        params = self.get_parameters(["car_name","wheels","width"])
        for param in params:
            self.get_logger().info("%s = %s" % (param.name,param.value))

        self.get_logger().info("car_name %d" % self.has_parameter("car_name"))
        self.get_logger().info("height %d" % self.has_parameter("height"))

    def update_param(self):
        self.get_logger().info("-------改-------")
        self.set_parameters([rclpy.Parameter("car_name",value="Mouse")])
        car_name = self.get_parameter("car_name")
        self.get_logger().info("修改后:%s = %s" % (car_name.name,car_name.value))

    def del_param(self):
        self.get_logger().info("-------删-------")
    
def main():
    rclpy.init()
    node = ParamServer()
    node.declare_param()
    node.get_param()
    node.update_param()
    node.del_param()
    
    rclpy.spin(node)
    rclpy.shutdown()
    
if __name__ == '__main__':
    main()