【ROS2 Humble】第二课 - ROS2库的基本使用

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

# 创建一个基于 Python 的ROS2功能包
ros2 pkg create --build-type ament_python <package_name>

# 创建一个基于 C++ 的ROS2功能包
ros2 pkg create --build-type ament_cmake <package_name>

# 上面两个指令创建的功能包缺少主入口文件，加上一个 --node-name 顺便把节点入口文件一起创建了
ros2 pkg create --build-type ament_python --node-name <node_name> <package_name>
ros2 pkg create --build-type ament_cmake --node-name <node_name> <package_name>

ros2 pkg create --build-type ament_python --node-name my_node my_pkg_python
ros2 pkg create --build-type ament_cmake --node-name my_node my_pkg_cmake

my_pkg_python/
├── my_pkg_python/          # 【重要】真正业务Python源码包（必须有__init__.py）
│   ├── __init__.py
│   └── my_node.py
├── resource/               # ament索引空文件，编译要用，初学不用管
│   └── my_pkg_python
├── test/                   # 单元测试，可选，正式项目建议保留，初学不用管
│   ├── __init__.py
│   └── test_my_node.py
├── package.xml             # 【重要】ROS2 ament元数据
├── setup.py                # 【重要】setuptools打包主文件
└── setup.cfg               # 【重要】修改脚本输出路径适配ros2 run

my_pkg_cmake
├── CMakeLists.txt      # CMake构建脚本（核心编译配置）
├── include
│   └── my_pkg_cmake    # 头文件存放目录，惯例套一层包名文件夹，防止头文件名冲突
├── package.xml         # ROS2包元信息，和python包是同一种文件格式，但依赖/标签不一样
└── src
    └── my_node.cpp     # C++源代码实现文件

import rclpy
from rclpy.node import Node
import time
from std_msgs.msg import String		# topic话题要想输出字符串类型数据要用这个String对象

class HelloPublishNode(Node):
    def __init__(self, node_name):
        super().__init__(node_name)
        self.pub = self.create_publisher(String, 'HelloTopic', 10)
        self.timer = self.create_timer(1, self.timer_callback)

    def timer_callback(self):
        
        msg = String()		# 字符串必须要用这个String对象
        msg.data = time.asctime()	# 真的数据内容在这个String的data里面
        self.pub.publish(msg)
        self.get_logger().info(f'HelloTopic publishes message: {msg.data}')


def main(args=None):
	# main 函数这边创建节点都是固定写法了
    rclpy.init(args=args)
    node = HelloPublishNode("hello_publish_node")
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

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

class HelloSubscribeNode(Node):
    def __init__(self, node_name):
        super().__init__(node_name)
        self.sub = self.create_subscription(String, 'HelloTopic', self.hello_topic_callback, 10)

    def hello_topic_callback(self, msg: String):
		# 接收的时候，把String对象里面的data拿出来
        self.get_logger().info(f'HelloTopic receives message: {msg.data}')


def main(args=None):
    rclpy.init(args=args)
    node = HelloSubscribeNode("hello_subscribe_node")
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>	// ROS 2标准消息库中的字符串消息类型，用于在话题上发送/接收字符串数据
#include <time.h>					// C标准时间库，虽然本代码中未直接使用，但通常用于时间相关操作（如获取系统时间）

using namespace std::chrono_literals;	// C++标准库的时间字面量支持，允许使用500ms、1s等简洁的时间表示法

/**
 * @brief 演示发布者节点类
 * 继承自rclcpp::Node，是一个自定义的ROS 2节点
 */
class DemoNode : public rclcpp::Node{
public:
    /**
     * @brief 构造函数
     * 初始化节点名称、创建发布者和定时器
     */
    DemoNode() : Node("demo_pub_node"){		// C++语法，调用父类的构造函数
        // 使用RCLCPP_INFO宏打印INFO级别的日志信息，C++中要给node节点打印日志输出就得这样搞
        // this->get_logger()获取当前节点的日志记录器
        RCLCPP_INFO(this->get_logger(), "Pub node start successful.");
        
        // 创建发布者：
        // - 模板参数<std_msgs::msg::String>指定消息类型为字符串
        // - "/chatter"是话题名称
        // - 10是队列大小（缓存最近10条消息）
        // - this->create_publisher是Node类的成员函数
        pub_ = this->create_publisher<std_msgs::msg::String>("/chatter", 10);
        
        // 创建定时器：
        // - 500ms：触发周期，每隔500毫秒触发一次
        // - std::bind(&DemoNode::timer_cb, this)：绑定成员函数timer_cb作为回调
        // - this->create_wall_timer创建基于系统时钟的定时器
        timer_ = this->create_wall_timer(500ms, std::bind(&DemoNode::timer_cb, this));
    }

private:
    /**
     * @brief 定时器回调函数
     * 每500ms被调用一次，创建消息并通过发布者发送
     */
    void timer_cb(){
        // 创建字符串消息对象（栈上分配）
        auto msg = std_msgs::msg::String();
        
        // 设置消息的数据内容为"Hello World."
        msg.data = "Hello World.";

        // 打印日志，显示正在发布的内容
        // c_str()将std::string转为C风格字符串
        RCLCPP_INFO(this->get_logger(), "publish %s", msg.data.c_str());
        
        // 通过发布者发布消息
        // ->是智能指针的箭头运算符，调用成员函数
        pub_->publish(msg);
    }
    
    // 发布者智能指针：
    // - SharedPtr表示共享指针，自动管理内存
    // - 模板参数指定消息类型为std_msgs::msg::String
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
    
    // 定时器智能指针：
    // - TimerBase是定时器的基类
    // - SharedPtr管理定时器对象的生命周期
    rclcpp::TimerBase::SharedPtr timer_;
};

/**
 * @brief 程序入口函数
 * 初始化ROS 2、创建节点对象、进入事件循环、最后清理资源
 */
int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto mynode = std::make_shared<DemoNode>();

    // 进入事件循环：
    // - spin会阻塞当前线程，处理所有回调（定时器、订阅、服务等）
    // - 直到收到Ctrl+C或调用shutdown()才会退出
    rclcpp::spin(mynode);
    rclcpp::shutdown();
    return 0;
}

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <time.h>

using namespace std::chrono_literals;

/**
 * @brief 演示订阅者节点类
 * 继承自rclcpp::Node，用于接收并处理话题消息
 */
class DemoNode : public rclcpp::Node{
public:
    /**
     * @brief 构造函数
     * 初始化节点名称、创建订阅者
     */
    DemoNode() : Node("demo_sub_node"){
        // 打印node节点启动成功日志
        RCLCPP_INFO(this->get_logger(), "Sub node start successful.");
        
        /**
         * @brief 创建订阅者
         * 
         * 语法：create_subscription<消息类型>(话题名, 队列大小, 回调函数)
         * 
         * 参数详解：
         * - "/chatter"：要订阅的话题名称，必须与发布者的话题名称完全一致
         * - 10：队列大小（深度），当消息处理速度跟不上接收速度时，
         *       缓存最近10条消息，丢弃更早的消息
         * - std::bind(&DemoNode::sub_cb, this, std::placeholders::_1)：
         *       绑定成员函数sub_cb作为回调，std::placeholders::_1表示回调函数占位符，
         *       用于接收传入的消息参数
         * 
         * 返回值：rclcpp::Subscription智能指针，存储在成员变量sub_中
         */
        sub_ = this->create_subscription<std_msgs::msg::String>(
            "/chatter", 
            10, 
            std::bind(&DemoNode::sub_cb, this, std::placeholders::_1)
        );
    }

private:
    /**
     * @brief 订阅者回调函数
     * 每当接收到话题消息时被自动调用
     * 
     * @param msg 接收到的消息，使用SharedPtr（共享指针）传递
     *            类型为std_msgs::msg::String的智能指针
     *            使用->操作符访问成员，如msg->data
     * 
     * 注意：回调函数在ROS 2的执行器（Executor）中被调用，
     *       应尽量快速返回，避免长时间阻塞
     */
    void sub_cb(const std_msgs::msg::String::SharedPtr msg){
        // 使用RCLCPP_INFO打印接收到的消息
        // msg->data：获取消息中的字符串数据
        // c_str()：将std::string转换为C风格字符串用于格式化输出
        RCLCPP_INFO(this->get_logger(), "receive: %s", msg->data.c_str());
    }
    
    /**
     * @brief 订阅者智能指针
     * 
     * 类型：rclcpp::Subscription<消息类型>::SharedPtr
     * - SharedPtr是共享指针，自动管理内存，无需手动delete
     * - 使用模板参数指定消息类型为std_msgs::msg::String
     * - 在构造函数中通过create_subscription创建并赋值
     * - 成员变量会在对象销毁时自动释放
     */
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
	// 使用共享指针创建一个指向DemoNode节点的指针，这么做可以自动释放资源，避免内存泄露
    auto mynode = std::make_shared<DemoNode>();
    rclcpp::spin(mynode);
    rclcpp::shutdown();
    return 0;
}

cmake_minimum_required(VERSION 3.8)
project(witcher_pkg_cpp)

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

# 将常用的依赖都写到一起，后面可以通过${COMMON_ROS_DEPS}调用
set(COMMON_ROS_DEPS
  rclcpp
  std_msgs
)

# 这个包用到的依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

include_directories(
  ${PROJECT_SOURCE_DIR}/src
)

# 添加执行脚本的文件的地址，加了这个以后才能找到执行脚本所在位置
add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp)
add_executable(demo_sub src/demo_sub.cpp)
add_executable(demo_pub src/demo_pub.cpp)

target_include_directories(${PROJECT_NAME}_node PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

# 给每个文件添加依赖关系，不然会include不到
ament_target_dependencies(${PROJECT_NAME}_node ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_pub ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_sub ${COMMON_ROS_DEPS})

# 得在instal这里把脚本启动命令写上，才能通过ros2 run找到相关的命令
install(TARGETS ${PROJECT_NAME}_node demo_pub demo_sub
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

std::bind(&DemoNode::sub_cb, this, std::placeholders::_1)

import rclpy
from rclpy.node import Node
from witcher_pkg_interfaces.srv import AddType		# 这里导入了一个自定义的参数类型，见后面的内容介绍


class ServerNode(Node):
    def __init__(self, node_name):
        super().__init__(node_name)
		# 创建一个service服务
        self.server = self.create_service(AddType, "add_server", self.adder_callback)
	
	# 这个callback也是特殊，服务会把接收到的参数的传给回调函数，然后进行计算并返回完整的response
    def adder_callback(self, request, response):
        response.sum = request.a + request.b
        return response


def main(args=None):
    rclpy.init(args=args)
    node = ServerNode("adder_server_node")
    rclpy.spin(node)		// 创建一个服务的节点并循环运行

    node.destroy_node()
    rclpy.shutdown()

import sys
import rclpy
from rclpy.node import Node
from witcher_pkg_interfaces.srv import AddType		# 这里导入了一个自定义的参数类型，见后面的内容介绍


class ClientNode(Node):
    def __init__(self, node_name):
        super().__init__(node_name)
        self.client = self.create_client(AddType, "add_server")		# 创建一个客户端
        while not self.client.wait_for_service(0.5):				# 等待连接服务端，等待500ms一次
            self.get_logger().info("wait for service...")
        self.request = AddType.Request()			# 实例化一个传参类型的request的对象

    def send_request(self):
        self.request.a = int(sys.argv[1])			# 要想取出对象的参数，直接.就行了。
        self.request.b = int(sys.argv[2])			# 这两步是配置好参数
        self.future = self.client.call_async(self.request)	# 调用服务端进行计算，并将参数传入


def main(args=None):
    rclpy.init(args=args)
    node = ClientNode("add_client_node")	# 创建客户端节点
    node.send_request()						# 发送请求
    while rclpy.ok():						# 查询 ROS 上下文是否还活着
        rclpy.spin_once(node)				# 处理一轮事件，等待节点有请求发送回来
        if node.future.done():
            try:
                response = node.future.result()		# 这里会拿到服务端返回的结果
            except Exception as e:
                node.get_logger().error(e)
            else:
                node.get_logger().info(
                    f"{sys.argv[1]} + {sys.argv[2]} = {response.sum}"
                )

    node.destroy_node()
    rclpy.shutdown()

#include <rclcpp/rclcpp.hpp>

// ROS 2标准服务库中的SetBool服务类型
// 服务定义：请求（bool data），响应（bool success, string message）
#include <std_srvs/srv/set_bool.hpp>

// 类型别名：简化服务类型的书写
// 以后使用SetBool就等同于std_srvs::srv::SetBool
using SetBool = std_srvs::srv::SetBool;

/**
 * @brief 演示服务端节点类
 * 继承自rclcpp::Node，提供一个设置布尔值的服务
 */
class DemoSrvServer : public rclcpp::Node
{
public:
    /**
     * @brief 构造函数
     * 初始化节点名称、创建服务端
     */
    DemoSrvServer() : Node("demo_srv_server")
    {
        /**
         * @brief 创建服务端
         * 
         * 语法：create_service<服务类型>(服务名, 回调函数)
         * 
         * 参数详解：
         * - "/demo_set_bool"：服务名称，客户端通过此名称调用服务
         * - std::bind(&DemoSrvServer::service_cb, this, 
         *            std::placeholders::_1, std::placeholders::_2)：
         *   绑定成员函数service_cb作为回调
         *   - _1：占位符，表示第一个参数（请求对象 Request）
         *   - _2：占位符，表示第二个参数（响应对象 Response）
         * 
         * 返回值：rclcpp::Service智能指针，存储在成员变量server_中
         */
        server_ = this->create_service<SetBool>(
            "/demo_set_bool",
            std::bind(&DemoSrvServer::service_cb, this, 
                      std::placeholders::_1, std::placeholders::_2)
        );
        
        // 打印服务启动成功日志
        RCLCPP_INFO(this->get_logger(), "Service Server ready");
    }

private:
    /**
     * @brief 服务回调函数
     * 当客户端请求到达时自动被调用
     * 
     * @param req 请求对象（SharedPtr共享指针）
     *            类型：SetBool::Request::SharedPtr
     *            包含成员：req->data（bool类型，客户端传入的布尔值）
     * 
     * @param res 响应对象（SharedPtr共享指针）
     *            类型：SetBool::Response::SharedPtr
     *            包含成员：
     *            - res->success（bool类型，表示操作是否成功）
     *            - res->message（string类型，返回给客户端的信息）
     * 
     * 注意：此函数在ROS 2执行器中调用，应快速返回避免阻塞
     */
    void service_cb(const SetBool::Request::SharedPtr req, 
                    SetBool::Response::SharedPtr res)
    {
        // 打印接收到的请求数据（将bool转为整数%d显示：1为true，0为false）
        RCLCPP_INFO(this->get_logger(), "recv request: %d", req->data);
        
        // 设置响应：操作成功
        res->success = true;
        
        // 设置响应：返回提示信息
        res->message = "ok done";
    }
    
    /**
     * @brief 服务端智能指针
     * 
     * 类型：rclcpp::Service<服务类型>::SharedPtr
     * - SharedPtr是共享指针，自动管理内存
     * - 使用模板参数指定服务类型为SetBool
     * - 在构造函数中通过create_service创建并赋值
     */
    rclcpp::Service<SetBool>::SharedPtr server_;
};

/**
 * @brief 程序入口函数
 */
int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<DemoSrvServer>());
    rclcpp::shutdown();
    
    return 0;
}

#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/set_bool.hpp>
using SetBool = std_srvs::srv::SetBool;
using namespace std::chrono_literals;

class DemoSrvClient : public rclcpp::Node {
public:
    DemoSrvClient(const std::string &node_name) : Node(node_name) {
        // 创建服务客户端：连接到名为 "/demo_set_bool" 的服务
        // this->create_client<服务类型>("服务名")
        client_ = this->create_client<SetBool>("/demo_set_bool");
        RCLCPP_INFO(this->get_logger(), "Client node created.");
        
        // 调用私有方法，实际发送请求（注意：这里在构造函数中同步执行，会阻塞构造完成）
        this->send_req();
    }

private:
    void send_req() {
        // ----- 步骤1：等待服务上线 -----
        // client_->wait_for_service(超时时间) 会阻塞等待，超时返回 false
        while (!client_->wait_for_service(1s)) {
            RCLCPP_WARN(this->get_logger(), "waiting service ...");
        }
        RCLCPP_INFO(this->get_logger(), "Client node connected.");

        // ----- 步骤2：构造请求 -----
        auto req = std::make_shared<SetBool::Request>();
        req->data = true;   // 设置请求数据为 true

        // ----- 步骤3：异步发送请求 -----
        auto result = client_->async_send_request(req);

        // ----- 步骤4：等待响应完成（同步阻塞） -----
        // rclcpp::spin_until_future_complete 会在等待 future 完成的同时，处理 ROS 2 回调
        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) {
            
            // ----- 步骤5：获取响应并打印 -----
            // result.future.get() 只能调用一次！获取响应对象（Response 的共享指针）
            auto res = result.future.get();
            
            // 打印响应信息：message 字符串和 success 布尔值（%d 显示 0 或 1）
            RCLCPP_INFO(this->get_logger(), "res: msg=%s", res->message.c_str());
            RCLCPP_INFO(this->get_logger(), "res: success=%d", res->success);
        } else {
            // 如果超时或中断，打印错误信息（可选）
            RCLCPP_ERROR(this->get_logger(), "Request failed or timed out.");
        }
    }

    /**
     * @brief 客户端智能指针
     * 类型：rclcpp::Client<SetBool>::SharedPtr
     * 用于调用服务，由 create_client 创建，自动管理生命周期
     */
    rclcpp::Client<SetBool>::SharedPtr client_;
};

/**
 * @brief 主函数
 * 
 * 流程：
 * 1. 初始化 ROS 2
 * 2. 创建客户端节点（构造时自动发送请求并等待响应）
 * 3. 由于请求在构造函数中同步完成，无需调用 spin，直接关闭
 * 4. 关闭 ROS 2
 */
int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<DemoSrvClient>("ClientNode");
    
    // 注意：这里没有调用 rclcpp::spin(node)，
    // 因为请求已经同步完成，在send_req的rclcpp::spin_until_future_complete中已经阻塞，并等待回调了
    
    rclcpp::shutdown();
    return 0;
}

cmake_minimum_required(VERSION 3.8)
project(witcher_pkg_cpp)

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(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)

set(COMMON_ROS_DEPS
  rclcpp
  std_msgs
  std_srvs
)

include_directories(
  ${PROJECT_SOURCE_DIR}/src
)

add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp)
add_executable(demo_sub src/demo_sub.cpp)
add_executable(demo_pub src/demo_pub.cpp)
add_executable(demo_server src/demo_server.cpp)
add_executable(demo_client src/demo_client.cpp)

target_include_directories(${PROJECT_NAME}_node PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

ament_target_dependencies(${PROJECT_NAME}_node ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_sub ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_pub ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_server ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_client ${COMMON_ROS_DEPS})

install(TARGETS ${PROJECT_NAME}_node demo_sub demo_pub demo_client demo_server
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

# 所在目录：/opt/ros/humble/share/std_msgs/msg/String.msg

# This was originally provided as an example message.
# It is deprecated as of Foxy
# It is recommended to create your own semantically meaningful message.
# However if you would like to continue using this please use the equivalent in example_msgs.

string data

bool data # e.g. for hardware enabling / disabling
---
bool success   # indicate successful run of triggered service
string message # informational, e.g. for error messages

ros2 interface list					// 查看全部已定义的接口
ros2 interface show [接口名称]		// 查看接口的完整定义
ros2 interface package [包名]		// 查看某个功能包定义的所有通信接口

ros2 pkg create --build_type ament_cmake [pkg包名] --dependencies rclpy rosidl_default_generators​

.
├── CMakeLists.txt
├── include
│   └── witcher_pkg_interfaces
├── package.xml
├── src
└── srv
    └── AddType.srv

int32 a
int32 b
---
int32 sum

cmake_minimum_required(VERSION 3.8)
project(witcher_pkg_interfaces)

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)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rosidl_default_generators REQUIRED)

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()

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/AddType.srv"
)

ament_package()

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

cd ~/ros2_ws/src/witcher_pkg_cpp
mkdir -p msg
touch msg/Book.msg

string name
string author
int32 price

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

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

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <witcher_pkg_cpp/msg/book.hpp>
#include <time.h>

using namespace std::chrono_literals;

class DemoNode : public rclcpp::Node{
public:
    DemoNode() : Node("demo_pub_node"){
        RCLCPP_INFO(this->get_logger(), "Pub node start successful.");
        pub_ = this->create_publisher<witcher_pkg_cpp::msg::Book>("/chatter", 10);
        timer_ = this->create_wall_timer(500ms, std::bind(&DemoNode::timer_cb, this));
        book.name = "C++ Primer";
        book.author = "Stanley B. Lippman";
        book.price = 109;
    }

private:
    void timer_cb(){
        pub_->publish(book);
        RCLCPP_INFO(this->get_logger(), "publish time: %d", count++);
    }
    int count = 0;
    witcher_pkg_cpp::msg::Book book = witcher_pkg_cpp::msg::Book();
    rclcpp::Publisher<witcher_pkg_cpp::msg::Book>::SharedPtr pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

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

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <witcher_pkg_cpp/msg/book.hpp>
#include <time.h>

using namespace std::chrono_literals;

class DemoNode : public rclcpp::Node{
public:
    DemoNode() : Node("demo_sub_node"){
        RCLCPP_INFO(this->get_logger(), "Sub node start successful.");
        sub_ = this->create_subscription<witcher_pkg_cpp::msg::Book>("/chatter", 10, std::bind(&DemoNode::sub_cb, this, std::placeholders::_1));
    }

private:
    void sub_cb(const witcher_pkg_cpp::msg::Book::SharedPtr book){
        RCLCPP_INFO(this->get_logger(), "receive book, name: %s, author: %s, price: %d.", book->name, book->author, book->price);
    }
    rclcpp::Subscription<witcher_pkg_cpp::msg::Book>::SharedPtr sub_;
};

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

bool enable		# goal/Goal()，这个应该就是固定写法，最上面这行固定就是goal，下面也是固定已经定义好的名字
---
bool finish		# result/Result()
---
int32 state		# feedback/Feedback()

import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from witcher_pkg_interfaces.action import MoveCircle		# 自己创建的action类型
import time
from random import randrange


class ActionServerNode(Node):
    def __init__(self, node_name):
        super().__init__(node_name)
        self._action_server = ActionServer(
            self, MoveCircle, "move_circle", self.move_circle_callback
        )
        self.get_logger().info("action server init succeed.")	# 显示一下action服务初始化的情况
	
    def move_circle_callback(self, goal_handle):
        self.get_logger().info("moving circle")
        feedback_msg = MoveCircle.Feedback()

        for i in range(0, 360, 5):
            if i % 30 == 0:
                feedback_msg.state = i
                goal_handle.publish_feedback(feedback_msg)
            time.sleep(randrange(2, 8, 1) / 50)

        goal_handle.succeed()
        result = MoveCircle.Result()
        result.finish = True
        return result


def main(args=None):
    rclpy.init(args=args)
    node = ActionServerNode("action_server_node")
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient

from witcher_pkg_interfaces.action import MoveCircle


class ActionClientNode(Node):
    def __init__(self, node_name):
        super().__init__(node_name)
        self._action_client = ActionClient(self, MoveCircle, "move_circle")
        self.get_logger().info("action client init succeed.")

    def send_move_circle(self, enable):
        goal_msg = MoveCircle.Goal()
        goal_msg.enable = enable
        self._action_client.wait_for_server()

        self.future = self._action_client.send_goal_async(
            goal_msg, feedback_callback=self.move_circle_feedback_callback
        )

        self.future.add_done_callback(self.move_circle_done_callback)

    def move_circle_feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(f"receive feedback: state = {feedback.state}")

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info(f"result: {result.finish}")

    def move_circle_done_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info("Goal rejected")
            return

        self.get_logger().info("Goal accepted")

        self.reuslt_future = goal_handle.get_result_async()
        self.reuslt_future.add_done_callback(self.get_result_callback)


def main(args=None):
    rclpy.init(args=args)
    node = ActionClientNode("action_node")
    node.send_move_circle(True)

    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

#include <rclcpp/rclcpp.hpp>

// ROS 2 Action 库头文件，提供 Action 服务端/客户端的创建和操作
#include <rclcpp_action/rclcpp_action.hpp>

// 自定义 Action 接口：MoveCircle（定义在 witcher_pkg_interfaces 包中）
#include <witcher_pkg_interfaces/action/move_circle.hpp>

// C 标准线程库（此处的 threads.h 实际应为 thread，但代码中用 sleep 函数，依赖 time.h）
// 注意：实际编译时需要 #include <thread> 和 #include <chrono>，但此处保留了原样
#include <threads.h>
#include <time.h>

// 类型别名：简化书写，之后使用 MoveCircle 代替完整类型名
using MoveCircle = witcher_pkg_interfaces::action::MoveCircle;

/**
 * @brief Action 服务端节点类
 * 
 * 功能：提供 MoveCircle Action 服务，模拟执行画圆动作（共 360 步）
 * - 支持目标接受/拒绝
 * - 支持中途取消
 * - 每 3 步发送一次进度反馈
 */
class ActionServerNode : public rclcpp::Node {
public:
    /**
     * @brief 构造函数
     * @param node_name 节点名称（外部传入）
     * 
     * 创建 Action 服务端，并绑定三个核心回调函数：
     * 1. handle_goal   - 处理目标请求（决定接受或拒绝）
     * 2. handle_cancel - 处理取消请求
     * 3. handle_accept - 目标接受后启动实际执行线程
     */
    ActionServerNode(std::string node_name) : Node(node_name) {
        // 创建 Action 服务端
        // 参数：this（节点指针）, 动作名称, 三个回调函数绑定，这三个回调函数是固定顺序，而且必须要配置
        server_ = rclcpp_action::create_server<MoveCircle>(
            this,
            "move_circle_action",		// Action 服务名称
            std::bind(&ActionServerNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),	// 目标处理回调
            std::bind(&ActionServerNode::handle_cancle, this, std::placeholders::_1),						// 取消处理回调
            std::bind(&ActionServerNode::handle_accept, this, std::placeholders::_1)                        // 接受后执行回调
        );
        RCLCPP_INFO(this->get_logger(), "Action server created successful.");
    }

private:
    /**
     * @brief ① 目标处理回调
     * 
     * 当客户端发送目标时被调用，用于决定是否接受该目标
     * 
     * @param uuid 目标的唯一标识符（此处未使用，用 (void)uuid 消除编译警告）
     * @param goal 客户端发送的目标对象（包含 enable 字段）
     * @return GoalResponse 接受/拒绝该目标
     * 
     * 逻辑：如果 goal->enable 为 true，则接受并立即准备执行；
     *       否则拒绝该目标
     */
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID &uuid,		// 只读的，使用&引用可以减少一次拷贝的开销，这是这个回调函数必须配置的。
        std::shared_ptr<const MoveCircle::Goal> goal
    ) {
        (void)uuid;  // 显式忽略未使用的参数，避免编译警告

        // 根据 enable 字段决定是否接受目标
        if (goal->enable) {
            return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;  // 接受并执行
        } else {
            return rclcpp_action::GoalResponse::REJECT;              // 拒绝目标
        }
    }

    /**
     * @brief ② 取消处理回调
     * 
     * 当客户端请求取消正在执行的目标时被调用
     * 
     * @param goal_handle 当前正在执行的目标句柄
     * @return CancelResponse 是否接受取消请求
     * 
     * 逻辑：始终接受取消请求（实际取消操作在 execute 函数中处理）
     */
    rclcpp_action::CancelResponse handle_cancle(
        const std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveCircle>> goal_handle	// 也使用了const，表示只读的意思
    ) {
        RCLCPP_INFO(this->get_logger(), "Cancel request received.");
        return rclcpp_action::CancelResponse::ACCEPT;  // 接受取消请求
    }

    /**
     * @brief ③ 接受目标后的执行启动回调
     * 
     * 当 handle_goal 返回 ACCEPT_AND_EXECUTE 后被调用
     * 此函数不能阻塞，因此需要创建新线程来执行实际任务
     * 
     * @param goal_handle 已接受的目标句柄
     * 
     * 实现：使用 std::thread 启动独立线程执行 execute 函数
     * .detach() 使线程与主线程分离，独立运行
     */
    void handle_accept(
        const std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveCircle>> goal_handle
    ) {
        // 创建新线程执行 execute 函数，并将 goal_handle 传入
        // detach() 让线程在后台独立运行，不阻塞主线程
        std::thread{std::bind(&ActionServerNode::execute, this, std::placeholders::_1), goal_handle}.detach();
    }

    /**
     * @brief ④ 实际动作执行函数（在新线程中运行）
     * 
     * 模拟画圆动作：从 1° 到 360°，每步耗时 1 秒
     * - 每 3° 发送一次进度反馈（state = 当前角度）
     * - 每步检查是否收到取消请求，若收到则提前终止
     * - 完成后返回结果（finish = true）
     * 
     * @param goal_handle 当前执行的目标句柄
     * 
     * 流程：
     * 1. 获取目标数据（goal->enable）
     * 2. 循环 1~360 度
     * 3. 检查取消标志，若取消则设置 finish=false 并返回
     * 4. 每 3 步发送一次反馈（feedback->state = i）
     * 5. 循环结束后设置 finish=true 并标记成功
     */
    void execute(
        const std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveCircle>> goal_handle
    ) {
        RCLCPP_INFO(this->get_logger(), "Executing action...");

        // 获取客户端传入的目标参数（goal 包含 enable 字段）
        auto goal = goal_handle->get_goal();

        // 创建反馈对象和结果对象（共享指针）
        auto feedback = std::make_shared<MoveCircle::Feedback>();
        auto result = std::make_shared<MoveCircle::Result>();

        // 模拟画圆：从 1° 到 360° 循环
        for (int i = 1; i <= 360; i++) {
            RCLCPP_INFO(this->get_logger(), "enter for %d...", i);

            // 【取消检查】每次循环检查是否收到取消请求
            if (goal_handle->is_canceling()) {
                RCLCPP_WARN(this->get_logger(), "Received cancle command.");
                result->finish = false;           // 设置结果为未完成
                goal_handle->canceled(result);    // 标记目标为已取消
                return;                           // 立即退出函数
            }

            // 【反馈发送】每 3 步发送一次进度反馈
            if (i % 3 == 0) {
                RCLCPP_INFO(this->get_logger(), "feedback %d", i);
                feedback->state = i;              // 反馈字段：当前角度
                goal_handle->publish_feedback(feedback);  // 发布反馈给客户端
            }

            sleep(1);  // 模拟每步耗时 1 秒（实际控制周期）
        }

        // 循环正常结束，表示动作完成
        result->finish = true;           // 设置结果为成功
        goal_handle->succeed(result);    // 标记目标为成功完成
    }

    /**
     * @brief Action 服务端智能指针
     * 
     * 类型：rclcpp_action::Server<MoveCircle>::SharedPtr
     * - 管理 Action 服务端的生命周期
     * - 在构造函数中通过 create_server 创建并赋值
     */
    rclcpp_action::Server<MoveCircle>::SharedPtr server_;
};

/**
 * @brief 程序入口函数
 * 
 * 流程：
 * 1. 初始化 ROS 2
 * 2. 创建 Action 服务端节点
 * 3. 进入事件循环（阻塞，处理回调请求）
 * 4. 收到 Ctrl+C 后退出循环，关闭 ROS 2
 */
int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ActionServerNode>("move_circle_action_server");
    rclcpp::spin(node);   // 阻塞等待，直到收到关闭信号

    rclcpp::shutdown();
    return 0;
}

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <witcher_pkg_interfaces/action/move_circle.hpp>
using MoveCircle = witcher_pkg_interfaces::action::MoveCircle;
using namespace std::chrono_literals;

/**
 * @brief Action 客户端节点类
 * 
 * 功能：连接到 Action 服务端，发送画圆任务目标
 * - 等待服务端上线
 * - 发送目标（enable = true）
 * - 接收服务端反馈（当前角度 state）
 * - 接收最终结果（finish 是否成功）
 * - 结果返回后自动关闭节点
 */
class ActionClientNode : public rclcpp::Node {
public:
    ActionClientNode(std::string node_name) : Node(node_name) {
        // 创建 Action 客户端
        // 参数：this（节点指针）, Action 服务名称
        client_ = rclcpp_action::create_client<MoveCircle>(
            this,
            "move_circle_action"
        );
        RCLCPP_INFO(this->get_logger(), "Action client created.");
        
        // 直接发送目标（内部会等待服务端上线）
        send_goal();
    }

private:
    /**
     * @brief 发送目标函数
     * 
     * 流程：
     * 1. 等待 Action 服务端上线（每 500ms 检查一次）
     * 2. 构造目标对象（设置 enable = true）
     * 3. 配置发送选项（绑定三个回调函数）
     * 4. 异步发送目标（不阻塞）
     */
    void send_goal() {
        // 【等待服务上线】
        // 循环等待，直到服务端可用（wait_for_action_server 超时返回 false）
        while (!client_->wait_for_action_server(500ms)) {
            RCLCPP_WARN(this->get_logger(), "Waiting for server...");
        }
        RCLCPP_INFO(this->get_logger(), "Server connected.");

        // 【构造目标】
        auto goal = MoveCircle::Goal();
        goal.enable = true;   // 启用画圆动作

        // 【配置发送选项】
        // 绑定三个回调函数：目标响应、反馈、结果
        auto goal_options = rclcpp_action::Client<MoveCircle>::SendGoalOptions();
        goal_options.goal_response_callback = 
            std::bind(&ActionClientNode::goal_response_callback, this, std::placeholders::_1);
        goal_options.feedback_callback = 
            std::bind(&ActionClientNode::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
        goal_options.result_callback = 
            std::bind(&ActionClientNode::result_callback, this, std::placeholders::_1);

        // 【异步发送目标】
        // 立即返回，不会阻塞，后续通过回调处理响应
        client_->async_send_goal(goal, goal_options);
    }

    /**
     * @brief ① 目标响应回调
     * 
     * 当服务端处理目标请求后被调用，告知目标是否被接受
     * 
     * @param goal_handle 目标句柄（若被拒绝则为 nullptr）
     * 
     * 逻辑：
     * - 若 goal_handle 为空 → 目标被拒绝，打印错误并关闭节点
     * - 否则打印成功信息，并保存句柄供后续使用
     */
    void goal_response_callback(
        const rclcpp_action::ClientGoalHandle<MoveCircle>::SharedPtr &goal_handle
    ) {
        if (!goal_handle) {
            // 目标被拒绝
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected.");
            rclcpp::shutdown();   // 关闭节点
            return;
        }

        RCLCPP_INFO(this->get_logger(), "Goal accepted");
        goal_handle_ = goal_handle;   // 保存句柄（可用于取消等操作）
    }

    /**
     * @brief ② 反馈回调
     * 
     * 当服务端发布反馈时被调用（在 execute 循环中每 3 度触发）
     * 
     * @param goal_handle 目标句柄（此处未使用，形参留空）
     * @param feedback 反馈对象（包含 state 字段，即当前角度）
     * 
     * 逻辑：打印当前进度（角度值）
     */
    void feedback_callback(
        rclcpp_action::ClientGoalHandle<MoveCircle>::SharedPtr,      // 未使用，可省略变量名
        const std::shared_ptr<const MoveCircle::Feedback> feedback
    ) {
        RCLCPP_INFO(this->get_logger(), "Received feedback, status: %d", feedback->state);
    }

    /**
     * @brief ③ 结果回调
     * 
     * 当动作执行完成（成功、取消或异常）时被调用
     * 
     * @param result WrappedResult 包含结果数据和状态码
     * 
     * 逻辑：
     * - 根据 result->result->finish 判断是否成功完成
     * - 打印相应信息
     * - 无论成功失败，都关闭节点（程序退出）
     */
    void result_callback(
        const rclcpp_action::ClientGoalHandle<MoveCircle>::WrappedResult &result
    ) {
        // 判断最终结果
        if (result.result->finish) {
            RCLCPP_INFO(this->get_logger(), "Goal run result is successful.");
        } else {
            RCLCPP_ERROR(this->get_logger(), "Goal run result is failure.");
        }
        rclcpp::shutdown();   // 收到结果后关闭节点
    }

    /**
     * @brief 客户端智能指针
     * 
     * 类型：rclcpp_action::Client<MoveCircle>::SharedPtr
     * 用于与 Action 服务端通信，由 create_client 创建
     */
    rclcpp_action::Client<MoveCircle>::SharedPtr client_;

    /**
     * @brief 目标句柄智能指针
     * 
     * 类型：rclcpp_action::ClientGoalHandle<MoveCircle>::SharedPtr
     * 用于标识当前正在执行的目标，可用于取消操作
     * 在 goal_response_callback 中赋值
     */
    rclcpp_action::ClientGoalHandle<MoveCircle>::SharedPtr goal_handle_;
};

/**
 * @brief 程序入口函数
 * 
 * 流程：
 * 1. 初始化 ROS 2
 * 2. 创建 Action 客户端节点（构造时自动发送目标）
 * 3. 进入事件循环（等待回调完成）
 * 4. 收到结果后，节点内部调用 shutdown()，退出循环
 * 5. 关闭 ROS 2
 */
int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ActionClientNode>("move_circle_client_node");
    rclcpp::spin(node);   // 阻塞等待，直到节点内部调用 rclcpp::shutdown()

    rclcpp::shutdown();
    return 0;
}

cmake_minimum_required(VERSION 3.8)
project(witcher_pkg_cpp)

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(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(witcher_pkg_cpp REQUIRED)
find_package(witcher_pkg_interfaces REQUIRED)
find_package(rosidl_default_generators REQUIRED)

set(COMMON_ROS_DEPS
  rclcpp
  rclcpp_action
  std_msgs
  std_srvs
  witcher_pkg_cpp
  witcher_pkg_interfaces
)

include_directories(
  ${PROJECT_SOURCE_DIR}/src
)

add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp)
add_executable(demo_sub src/demo_sub.cpp)
add_executable(demo_pub src/demo_pub.cpp)
add_executable(demo_server src/demo_server.cpp)
add_executable(demo_client src/demo_client.cpp)
add_executable(demo_pub_book src/demo_pub_book.cpp)
add_executable(demo_sub_book src/demo_sub_book.cpp)
add_executable(demo_action_server src/demo_action_server.cpp)
add_executable(demo_action_client src/demo_action_client.cpp)

target_include_directories(${PROJECT_NAME}_node PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

ament_target_dependencies(${PROJECT_NAME}_node ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_sub ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_pub ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_server ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_client ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_sub_book ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_pub_book ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_action_server ${COMMON_ROS_DEPS})
ament_target_dependencies(demo_action_client ${COMMON_ROS_DEPS})

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Book.msg"
)

install(TARGETS ${PROJECT_NAME}_node demo_sub demo_pub demo_client demo_server demo_sub_book demo_pub_book demo_action_server demo_action_client
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

ros2 param list		// 查看所有参数
ros2 param describe [节点名] [参数名]		// 查看参数详细描
ros2 param set [节点名] [参数名] [目标值]	// 修改参数名
ros2 param get [节点名] [参数名]			// 获取参数名
ros2 param dump [节点名]					// 输出节点的全部参数名称
ros2 param load [节点名]	[参数文件]		// 加载参数文件到对应节点

import rclpy
import rclpy.node

class MinimalParam(rclpy.node.Node):
    def __init__(self):
        super().__init__('minimal_param_node')

		# node节点使用declare_parameter声明一个参数与参数值，每需要一个参数就要声明一次
		# 这里声明一个名叫my_parameter的参数，值为world
        self.declare_parameter('my_parameter', 'world')

        self.timer = self.create_timer(1, self.timer_callback)

    def timer_callback(self):
		# 读取param参数，也是通过node节点的get_parameter来获取，记住写法就行
        my_param = self.get_parameter('my_parameter').get_parameter_value().string_value

        self.get_logger().info('Hello %s!' % my_param)
		
		# 这是要修改参数值，可以看得出是创建了一个新的参数值，然后下方通过set_parameters设置这个值
        my_new_param = rclpy.parameter.Parameter(
            'my_parameter',
            rclpy.Parameter.Type.STRING,
            'world'
        )
        all_new_parameters = [my_new_param]	# set_parameters应该是可以一次设置很多值，用列表包一下
        self.set_parameters(all_new_parameters)

def main():
    rclpy.init()
    node = MinimalParam()
    rclpy.spin(node)

if __name__ == '__main__':
    main()

#include <chrono>
#include <functional>
#include <string>

#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;

class MinimalParam : public rclcpp::Node
{
public:
  MinimalParam()
  : Node("minimal_param_node")
  {
	// 这里声明了一个可以被修改的参数
    this->declare_parameter("my_parameter", "world");

    timer_ = this->create_wall_timer(
      1000ms, std::bind(&MinimalParam::timer_callback, this));
  }

  void timer_callback()
  {
	// 这里获取参数的值
    std::string my_param = this->get_parameter("my_parameter").as_string();

    RCLCPP_INFO(this->get_logger(), "Hello %s!", my_param.c_str());

	// 如果要设置值，就新创建一个parameter，然后包成一个vector以后传入给set_parameters函数
    std::vector<rclcpp::Parameter> all_new_parameters{rclcpp::Parameter("my_parameter", "world")};
    this->set_parameters(all_new_parameters);
  }

private:
  rclcpp::TimerBase::SharedPtr timer_;
};

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

ros2 pkg create --build-type ament_cmake --license Apache-2.0 --dependencies pluginlib --node-name area_node polygon_base

#ifndef POLYGON_BASE_REGULAR_POLYGON_HPP
#define POLYGON_BASE_REGULAR_POLYGON_HPP

namespace polygon_base
{
  class RegularPolygon
  {
    public:
      virtual void initialize(double side_length) = 0;
      virtual double area() = 0;
      virtual ~RegularPolygon(){}

    protected:
      RegularPolygon(){}
  };
}  // namespace polygon_base

#endif  // POLYGON_BASE_REGULAR_POLYGON_HPP

# 库 (这将被用于插件的基类)
add_library(${PROJECT_NAME} INTERFACE)
add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME})
target_compile_features(${PROJECT_NAME} INTERFACE c_std_99 cxx_std_17)
target_include_directories(${PROJECT_NAME} INTERFACE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
)
target_link_libraries(${PROJECT_NAME} INTERFACE ${pluginlib_TARGETS})

# 将这个头文件安装到ROS2的include中，可以被任意文件引用
install(DIRECTORY include/
  DESTINATION include/${PROJECT_NAME}
)

# 安装库并导出到目标
install(TARGETS ${PROJECT_NAME}
  EXPORT export_${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)
install(EXPORT export_${PROJECT_NAME}
  NAMESPACE ${PROJECT_NAME}::
  DESTINATION share/${PROJECT_NAME}/cmake
)

# Export old-style CMake variables
ament_export_include_directories(
  include
)

# Export modern CMake targets
ament_export_targets(
  export_${PROJECT_NAME}
)

ros2 pkg create --build-type ament_cmake --license Apache-2.0 --dependencies polygon_base pluginlib --library-name polygon_plugins polygon_plugins

#include <polygon_base/regular_polygon.hpp>
#include <cmath>

namespace polygon_plugins
{
  class Square : public polygon_base::RegularPolygon
  {
    public:
      void initialize(double side_length) override
      {
        side_length_ = side_length;
      }

      double area() override
      {
        return side_length_ * side_length_;
      }

    protected:
      double side_length_;
  };

  class Triangle : public polygon_base::RegularPolygon
  {
    public:
      void initialize(double side_length) override
      {
        side_length_ = side_length;
      }

      double area() override
      {
        return 0.5 * side_length_ * getHeight();
      }

      double getHeight()
      {
        return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2)));
      }

    protected:
      double side_length_;
  };
}

// 注册插件：引入pluginlib库
#include <pluginlib/class_list_macros.hpp>
// 注册插件：通过PLUGINLIB_EXPORT_CLASS宏将正方形和三角形的插件编入系统插件库中
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon)
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon)

<library path="polygon_plugins">
  <class type="polygon_plugins::Square" base_class_type="polygon_base::RegularPolygon">
    <description>This is a square plugin.</description>
  </class>
  <class type="polygon_plugins::Triangle" base_class_type="polygon_base::RegularPolygon" name="awesome_triangle">
    <description>This is a triangle plugin.</description>
  </class>
</library>

pluginlib_export_plugin_description_file(polygon_base plugins.xml)

#include <pluginlib/class_loader.hpp>
#include <polygon_base/regular_polygon.hpp>

int main(int argc, char** argv)
{
  // To avoid unused parameter warnings
  (void) argc;
  (void) argv;

  pluginlib::ClassLoader<polygon_base::RegularPolygon> poly_loader("polygon_base", "polygon_base::RegularPolygon");

  try
  {
    std::shared_ptr<polygon_base::RegularPolygon> triangle = poly_loader.createSharedInstance("awesome_triangle");
    triangle->initialize(10.0);

    std::shared_ptr<polygon_base::RegularPolygon> square = poly_loader.createSharedInstance("polygon_plugins::Square");
    square->initialize(10.0);

    printf("Triangle area: %.2f\n", triangle->area());
    printf("Square area: %.2f\n", square->area());
  }
  catch(pluginlib::PluginlibException& ex)
  {
    printf("The plugin failed to load for some reason. Error: %s\n", ex.what());
  }

  return 0;
}

colcon build --packages-select polygon_base polygon_plugins

ros2 plugin list | grep polygon

ros2 run polygon_base area_node

// 结果
Triangle area: 43.30
Square area: 100.00