做一个巡逻小海龟，目标位置动态更新，轨迹也随之更新

一，自定义服务接口
chapt4/chapt4_ws/chapt4_interfaces/srv/新建文件
Patrol.srv

float32 target_x 	# 目标x值
float32 target_y 	# 目标y值
---
int8 SUCCESS = 1    # 定义常量，表示成功
int8 FAIL = 0		    # 定义常量，表示失败
int8 result  	      # 处理结果

注意—上面就是request（输入），—下面就算response（输出）
修改CMakeLists.txt
新增

rosidl_generate_interfaces(${PROJECT_NAME}
    "srv/FaceDetector.srv"
    #新增路径
    "srv/Patrol.srv"
    DEPENDENCIES sensor_msgs

)

编译

colcon build
source install/setup.bash

运行

ros2 interface show chapt4_interfaces/srv/Patrol 

二，构建服务端
实现提供x，y的值，小海龟能服务到指定位置去。
2.1在ros2/chapt4/chapt4_ws/src下创造公共空间

ros2 pkg create demo_cpp_service  --build-type ament_cmake  --dependencies chapt4_interfaces rclcpp geometry_msgs turtlesim  --license Apach-2.0
going to create a new package

在ros2/chapt4/chapt4_ws/src/demo_cpp_service/src下新建turtle_control.cpp文件

#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
using Patrol = chapt4_interfaces::srv::Patrol;
#include "rcl_interfaces/msg/set_parameters_result.hpp"
using SetParametersResult = rcl_interfaces::msg::SetParametersResult;


class TurtleController : public rclcpp::Node
{
public:
    TurtleController() : Node("turtle_controller")
    {
        velocity_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
            "/turtle1/cmd_vel", 10);
        pose_subscription_ = this->create_subscription<turtlesim::msg::Pose>(
            "/turtle1/pose", 10,
            std::bind(&TurtleController::on_pose_received_, this, std::placeholders::_1));
             // 3.创建服务
    patrol_server_ = this->create_service<Patrol>(
        "patrol",
        [&](const std::shared_ptr<Patrol::Request> request,
            std::shared_ptr<Patrol::Response> response) -> void {
          // 判断巡逻点是否在模拟器边界内
          if ((0 < request->target_x && request->target_x < 12.0f)
           && (0 < request->target_y && request->target_y < 12.0f)) {
            target_x_ = request->target_x;
            target_y_ = request->target_y;
            response->result = Patrol::Response::SUCCESS;
          }else{
            response->result = Patrol::Response::FAIL;
          }
        });
            // 声明和获取参数初始值
    this->declare_parameter("k", 1.0);
    this->declare_parameter("max_speed", 1.0);
    this->get_parameter("k", k_);
    this->get_parameter("max_speed", max_speed_);

     // 添加参数设置回调
    parameters_callback_handle_ = this->add_on_set_parameters_callback(
        [&](const std::vector<rclcpp::Parameter> &params)
        -> SetParametersResult {
            // 遍历参数
            for (auto param : params) {
                RCLCPP_INFO(this->get_logger(), "更新参数 %s 值为：%f",param.get_name().c_str(), param.as_double());
                if (param.get_name() == "k") {
                    k_ = param.as_double();
                } else if (param.get_name() == "max_speed") {
                    max_speed_ = param.as_double();
                }
            }
            auto result = SetParametersResult();
            result.successful = true;
            return result;
    });
    }

private:
    void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose) {
    auto message = geometry_msgs::msg::Twist();
    // 1.记录当前位置
    double current_x = pose->x;
    double current_y = pose->y;
    RCLCPP_INFO(this->get_logger(), "当前位置:(x=%f,y=%f)", current_x, current_y);

    // 2.计算距离目标的距离，与当前海龟朝向的角度差
    double distance =
        std::sqrt((target_x_ - current_x) * (target_x_ - current_x) +
                  (target_y_ - current_y) * (target_y_ - current_y));
    double angle =
        std::atan2(target_y_ - current_y, target_x_ - current_x) - pose->theta;

    // 3.控制策略：距离大于0.1继续运动，角度差大于0.2则原地旋转，否则直行
    if (distance > 0.1) {
      if(fabs(angle)>0.2)
      {
        message.angular.z = fabs(angle);
      }else{
        // 通过比例控制器计算输出速度
        message.linear.x = k_ * distance;

      }
    }

    // 4.限制最大值并发布消息
    if (message.linear.x > max_speed_) {
       message.linear.x = max_speed_;
    }
    velocity_publisher_->publish(message);
  }


private:
  // 2.添加 Patrol 类型服务共享指针 patrol_server_ 为成员变量
  rclcpp::Service<Patrol>::SharedPtr patrol_server_;

    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscription_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher_;
    double target_x_{1.0};  // 目标位置X,设置默认值1.0
    double target_y_{1.0};  // 目标位置Y,设置默认值1.0
    double k_{1.0};         // 比例系数，控制输出=误差*比例系数
    double max_speed_{3.0}; // 最大线速度，设置默认值3.0
  OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle_;

};

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

如果环境有问题
步骤1，按F1键
步骤2，输入C/C++: Edit Configurations (UI)
步骤3，找到包含路径，添加
/usr/include/x86_64-linux-gnu/qt5/**
整体效果如下：

${workspaceFolder}/**
/opt/ros/humble/include/**
/usr/include/x86_64-linux-gnu/qt5/**

修改CMakeLists.txt文件

cmake_minimum_required(VERSION 3.8)
project(demo_cpp_service)

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(chapt4_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)

#构建turtle_control文件
add_executable(turtle_control  src/turtle_control.cpp)
ament_target_dependencies(turtle_control rclcpp geometry_msgs turtlesim chapt4_interfaces)

install(TARGETS
  turtle_control
  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()

在ros2/chapt4/chapt4_ws/下开始构建

colcon build
source install/setup.bash

先运行小海龟模拟器

ros2 run turtlesim turtlesim_node 

在新的终端运行服务器

ros2 run demo_cpp_service turtle_control 

在ros2/chapt4/chapt4_ws/下新的中终端
运行

source install/setup.bash

打开qt控制界面

rqt

选中/partol

可以实时修改x，y的值作为目标值，小海龟会运动到指定位置

三，构建客户端（随机生成x，y，让小海龟自动巡逻）
在ros2/chapt4/chapt4_ws/src/demo_cpp_service/src下新建patrol_client.cpp
代码如下：

#include <chrono>
#include <cstdlib>
#include <ctime>
#include "rclcpp/rclcpp.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
#include <chrono> // 引入时间相关头文件
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/msg/parameter_value.hpp"
#include "rcl_interfaces/msg/parameter_type.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"

// 使用时间单位的字面量，可以在代码中使用 s 和 ms 表示时间
using namespace std::chrono_literals;
using Patrol = chapt4_interfaces::srv::Patrol;
using SetP = rcl_interfaces::srv::SetParameters;

class PatrolClient : public rclcpp::Node
{
public:
  //构造函数
  PatrolClient() : Node("patrol_client")
  {
    patrol_client_ = this->create_client<Patrol>("patrol");//服务端的名称
    timer_ = this->create_wall_timer(10s, std::bind(&PatrolClient::timer_callback, this));
    srand(time(NULL)); // 初始化随机数种子，使用当前时间作为种子
  }
  void update_server_param_k(double k)
  {
    // 1.创建一个参数对象
    auto param = rcl_interfaces::msg::Parameter();
    param.name = "k";
    // 2.创建参数值对象并赋值
    auto param_value = rcl_interfaces::msg::ParameterValue();
    param_value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
    param_value.double_value = k;
    param.value = param_value;
    // 3.请求更新参数并处理
    auto response = call_set_parameters(param);
    if (response == nullptr)
    {
      RCLCPP_WARN(this->get_logger(), "参数修改失败");
      return;
    }
    else
    {
      for (auto result : response->results)
      {
        if (result.successful)
        {
          RCLCPP_INFO(this->get_logger(), "参数k 已修改为：%f", k);
        }
        else
        {
          RCLCPP_WARN(this->get_logger(), "参数k 失败原因：%s", result.reason.c_str());
        }
      }
    }
  }

  std::shared_ptr<SetP::Response> call_set_parameters(
      rcl_interfaces::msg::Parameter &parameter)
  {
    // 1. 创建客户端等待服务上线
    auto param_client = this->create_client<SetP>(
        "/turtle_controller/set_parameters");
    while (!param_client->wait_for_service(std::chrono::seconds(1)))
    {
      if (!rclcpp::ok())
      {
        RCLCPP_ERROR(this->get_logger(), "等待服务的过程中被打断...");
        return nullptr;
      }
      RCLCPP_INFO(this->get_logger(), "等待参数设置服务端上线中");
    }
    // 2. 创建请求对象
    auto request =
        std::make_shared<SetP::Request>();
    request->parameters.push_back(parameter);
    // 3. 异步调用、等待并返回响应结果
    auto future = param_client->async_send_request(request);
    rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
    auto response = future.get();
    return response;
  }

  void timer_callback()
  {
    // 1.等待服务端上线
    while (!patrol_client_->wait_for_service(1s))
    {
      // 等待时检测rclcpp的状态
      if (!rclcpp::ok())
      {
        RCLCPP_ERROR(this->get_logger(), "等待服务的过程中被打断...");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "等待服务端上线中");
    }
    // 2.构造请求的
    auto request = std::make_shared<Patrol::Request>();
    request->target_x = rand() % 15;
    request->target_y = rand() % 15;
    RCLCPP_INFO(this->get_logger(), "请求巡逻：(%f,%f)", request->target_x, request->target_y);
    // 3.发送异步请求，然后等待返回，返回时调用回调函数
    patrol_client_->async_send_request(
        request,
        [&](rclcpp::Client<Patrol>::SharedFuture result_future) -> void
        {
          auto response = result_future.get();
          if (response->result == Patrol::Response::SUCCESS)
          {
            RCLCPP_INFO(this->get_logger(), "目标点处理成功");
          }
          else if (response->result == Patrol::Response::FAIL)
          {
            RCLCPP_INFO(this->get_logger(), "目标点处理失败");
          }
        });
  }

private:
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Client<Patrol>::SharedPtr patrol_client_;
};
int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<PatrolClient>();//执行构造函数，但是timer_callback定时器已创建，但还没开始计时
  //k用于线性或角速度的比例控制器：速度 = k * 误差。调整 k 会改变机器人响应目标点的灵敏程度或到达目标点的行为特征。
  node->update_server_param_k(1.5);
  rclcpp::spin(node);//这时事件循环开始运行，定时器才会真正开始计时，每 10 秒触发一次 timer_callback
  rclcpp::shutdown();
  return 0;
}

整个启动流程
main()
└─ std::make_shared()
└─ PatrolClient::PatrolClient() ← 先执行构造函数，timer_callback定时器已创建，但还没开始计时
└─ node->update_server_param_k(1.5) ← 后执行k用于线性或角速度的比例控制器
└─ rclcpp::spin(node)← timer_callback定时器，才开始每 10 秒触发一次

修改CMakeLists.txt文件
修改部分：

#构建turtle_control文件
add_executable(turtle_control  src/turtle_control.cpp)
ament_target_dependencies(turtle_control rclcpp geometry_msgs turtlesim chapt4_interfaces)
#构建patrol_client文件
add_executable(patrol_client  src/patrol_client.cpp)
ament_target_dependencies(patrol_client rclcpp geometry_msgs turtlesim chapt4_interfaces)


install(TARGETS
  turtle_control
  patrol_client
  DESTINATION lib/${PROJECT_NAME}
)

然后开始构建
在ros2/chapt4/chapt4_ws/下运行

colcon build
source install/setup.bash

步骤1,在【终端1】先运行小海龟模拟器

ros2 run demo_cpp_service turtle_control 

步骤2,在【终端2】先运行服务端
在ros2/chapt4/chapt4_ws/下运行

source install/setup.bash

ros2 run demo_cpp_service turtle_control 

步骤3,在【终端3】运行客户端

source install/setup.bash

ros2 run demo_cpp_service patrol_client 

最终结果

补充修改节点参数的几个场景

我们本次演示主要是场景2,节点运行其他节点修改自己的参数
服务端的主要代码

#include "rcl_interfaces/msg/set_parameters_result.hpp" // SetParametersResult 消息头文件，用于取回调结果
using SetParametersResult = rcl_interfaces::msg::SetParametersResult;//用于回调

 //--------------------节点参数修改关键部分-------------------------------------------
                // 声明和获取参数初始值
        this->declare_parameter("k", 1.0);//注册参数 k，默认值 1.0，允许外部动态修改
        this->declare_parameter("max_speed", 1.0);//注册参数 max_speed，默认值 1.0
        this->get_parameter("k", k_);//把参数 k 的当前值读入成员变量 k_，用k_待存即将修改的值，为后续回调修改作准备
        this->get_parameter("max_speed", max_speed_);//同上

        // 添加参数设置回调
        //如果没有回调，客户端能不能修改？能修改（调用 /turtle_controller/set_parameters 会成功），但没有实际效果，因为 k_ 没变。只有添加了回调，修改才能实时生效。
        parameters_callback_handle_ = this->add_on_set_parameters_callback(
            [&](const std::vector<rclcpp::Parameter> &params)
            -> SetParametersResult {
                // 遍历参数
                for (auto param : params) {
                    RCLCPP_INFO(this->get_logger(), "更新参数 %s 值为：%f",param.get_name().c_str(), param.as_double());
                    if (param.get_name() == "k") {
                        k_ = param.as_double();
                    } else if (param.get_name() == "max_speed") {
                        max_speed_ = param.as_double();
                    }
                }
                auto result = SetParametersResult();
                result.successful = true;
                return result;
        });
      //--------------------节点参数修改关键部分-------------------------------------------

客户端主要代码
main函数里面

  node->update_server_param_k(1.5);

其对应的update_server_param_k函数代码

// 客户端修改服务端参数的核心函数
void update_server_param_k(double k)
{
    // 1. 创建参数对象，设置名称、类型、值
    auto param = rcl_interfaces::msg::Parameter();
    param.name = "k";
    auto param_value = rcl_interfaces::msg::ParameterValue();
    param_value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
    param_value.double_value = k;
    param.value = param_value;

    // 2. 创建针对目标节点的 SetParameters 客户端
    auto param_client = this->create_client<SetP>("/turtle_controller/set_parameters");

    // 3. 等待服务端上线
    while (!param_client->wait_for_service(std::chrono::seconds(1))) {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(this->get_logger(), "等待服务被打断");
            return;
        }
        RCLCPP_INFO(this->get_logger(), "等待 /turtle_controller/set_parameters 服务上线...");
    }

    // 4. 构造请求，将参数加入列表
    auto request = std::make_shared<SetP::Request>();
    request->parameters.push_back(param);

    // 5. 异步发送请求，并阻塞等待响应
    auto future = param_client->async_send_request(request);
    rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
    auto response = future.get();

    // 6. 处理响应结果
    if (response == nullptr) {
        RCLCPP_WARN(this->get_logger(), "参数修改失败");
    } else {
        for (auto &result : response->results) {
            if (result.successful) {
                RCLCPP_INFO(this->get_logger(), "参数 k 已修改为：%f", k);
            } else {
                RCLCPP_WARN(this->get_logger(), "参数 k 修改失败，原因：%s", result.reason.c_str());
            }
        }
    }
}

除了参数需要注意的，同时，为了完成这个项目要开三个终端，能不能集成在一个文件中呢？ 有的兄弟，有的 接下来介绍launch

在ros2/chapt4/chapt4_ws/demo_cpp_service/下新建launch文件
再新建demo.launch.py文件
代码如下

import launch
import launch_ros

def generate_launch_description():
    action_declare_arg_max_spped = launch.actions.DeclareLaunchArgument('launch_max_speed', default_value='2.0')

    action_node_turtle_control = launch_ros.actions.Node(
        #服务端
        package='demo_cpp_service',
        executable="turtle_control",
        output='screen',#输出显示在屏幕
        parameters=[{'max_speed': launch.substitutions.LaunchConfiguration(
  'launch_max_speed', default='2.0')}],

    )
    action_node_patrol_client = launch_ros.actions.Node(
        #客户端
        package='demo_cpp_service',
        executable="patrol_client",
        output='log',#输出日志
    )
    action_node_turtlesim_node = launch_ros.actions.Node(
        #开启小海龟
        package='turtlesim',
        executable='turtlesim_node',
        output='both',#输出在屏幕+日志
    )
   # 合成启动描述并返回
    launch_description = launch.LaunchDescription([
        action_declare_arg_max_spped,
        action_node_turtle_control,
        action_node_patrol_client,
        action_node_turtlesim_node
    ])
    return launch_description

修改CMakeLists.txt文件
新增：

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

在ros2/chapt4/chapt4_ws/
开始构建

colcon build

运行

ros2 launch demo_cpp_service demo.launch.py 

结果

一个终端就能实现服务端，客户端，小海龟模拟器三个节点。

launch的进阶版----包含：
动态参数控制：通过 DeclareLaunchArgument 允许用户在启动时选择是否打开 rqt。

复用其他 launch 文件：使用 IncludeLaunchDescription 启动 turtlesim 的多窗口示例。

条件执行：根据参数值决定是否运行某个进程。

延迟启动：利用 TimerAction 将关键动作推迟 2 秒执行，避免资源竞争。

动作分组：用 GroupAction 组织多个定时动作，使结构清晰。

在ros2/chapt4/chapt4_ws/demo_cpp_service/下新建launch文件
再新建actions.launch…py文件
代码如下：

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # ========== 1. 声明一个启动参数 ==========
    # 参数名为 'startup_rqt'，默认值为 'True'（字符串）
    # 用户可以通过命令行覆盖：ros2 launch ... startup_rqt:=False
    action_declare_startup_rqt = launch.actions.DeclareLaunchArgument(
        'startup_rqt', 
        default_value='True'
    )
    # 获取参数的实际值（运行时由用户传入或默认值），供后续条件判断使用
    startup_rqt = launch.substitutions.LaunchConfiguration('startup_rqt')

    # ========== 2. 准备“包含其他 launch 文件”的动作 ==========
    # 拼接 turtlesim 功能包中 multisim.launch.py 的完整路径
    multisim_launch_path = [
        get_package_share_directory('turtlesim'),  # 如 /opt/ros/humble/share/turtlesim
        '/launch/',
        'multisim.launch.py'
    ]
    # 该动作会启动另一个 launch 文件（通常用于启动两个乌龟窗口）
    action_include_launch = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            multisim_launch_path
        )
    )

    # ========== 3. 打印信息动作（仅演示，不影响业务） ==========
    # 将 multisim_launch_path 转换为字符串并输出到终端
    action_log_info = launch.actions.LogInfo(msg=str(multisim_launch_path))

    # ========== 4. 条件执行进程动作 ==========
    # 执行系统命令 'rqt'（ROS2 图形工具）
    # condition 判断：仅当 startup_rqt 的值为 'True'（不区分大小写）时才执行
    action_topic_list = launch.actions.ExecuteProcess(
        condition=launch.conditions.IfCondition(startup_rqt),
        cmd=['rqt'],
    )

    # ========== 5. 将多个动作分组，并添加延迟 ==========
    action_group = launch.actions.GroupAction([
        # 延迟 2 秒后执行 action_include_launch（启动两个乌龟）
        launch.actions.TimerAction(period=2.0, actions=[action_include_launch]),
        # 延迟 2 秒后执行 action_topic_list（启动 rqt）
        launch.actions.TimerAction(period=2.0, actions=[action_topic_list]),      
    ])

    # ========== 6. 返回整个启动描述 ==========
    return launch.LaunchDescription([
        action_declare_startup_rqt,   # 必须先声明参数
        action_log_info,              # 打印路径信息（可选）
        action_group                  # 执行分组动作（含定时器）
    ])

在ros2/chapt4/chapt4_ws/
开始构建

colcon build

运行

ros2 launch demo_cpp_service actions.launch.py 

结果，运行了’turtlesim’, ‘/launch/’, 'multisim.launch.py’的launch文件+rqt