ros2 server 可以设置命令同时获取位置

1. 一个自定义服务 SetCommandGetPose.srv：

   • 请求字段 float32 command

   • 响应字段 geometry_msgs/Pose pose

2. 服务端收到请求后，把 command 缓存下来，再把当前位姿填进响应返回。

3. 为了便于演示，位置用一个简单计数器模拟；你可以把它替换成 TF、里程计或 SLAM 输出。

---

一、创建功能包

bash

复制

ros2 pkg create --build-type ament_cmake pose_server_cpp \
  --dependencies rclcpp geometry_msgs

---

二、定义服务接口
在包目录里新建 srv/SetCommandGetPose.srv：

复制

float32 command
---
geometry_msgs/Pose pose

在 CMakeLists.txt 里加入：

cmake

复制

find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/SetCommandGetPose.srv"
  DEPENDENCIES geometry_msgs
)

ament_export_dependencies(rosidl_default_runtime)

在 package.xml 里加入：

xml

复制

<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<depend>geometry_msgs</depend>

---

三、服务端源码
把下面内容保存为 src/pose_server.cpp：

cpp

复制

#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "pose_server_cpp/srv/set_command_get_pose.hpp"  // 编译后生成的头文件

using namespace std::chrono_literals;

class PoseServer : public rclcpp::Node
{
public:
  PoseServer()
  : Node("pose_server")
  {
    pose_.position.x = 0.0;
    pose_.position.y = 0.0;
    pose_.position.z = 0.0;
    pose_.orientation.w = 1.0;

    srv_ = create_service<pose_server_cpp::srv::SetCommandGetPose>(
      "set_command_get_pose",
      std::bind(&PoseServer::handle_request, this,
                std::placeholders::_1, std::placeholders::_2));

    timer_ = create_wall_timer(
      100ms, std::bind(&PoseServer::update_pose, this));

    RCLCPP_INFO(this->get_logger(), "Pose Server ready.");
  }

private:
  void update_pose()
  {
    // 模拟 x 方向匀速运动
    pose_.position.x += 0.01;
  }

  void handle_request(
    const std::shared_ptr<pose_server_cpp::srv::SetCommandGetPose::Request> request,
    std::shared_ptr<pose_server_cpp::srv::SetCommandGetPose::Response>      response)
  {
    command_ = request->command;
    RCLCPP_INFO(this->get_logger(), "Received command: %.2f", command_);

    response->pose = pose_;
  }

  rclcpp::Service<pose_server_cpp::srv::SetCommandGetPose>::SharedPtr srv_;
  rclcpp::TimerBase::SharedPtr timer_;
  geometry_msgs::msg::Pose pose_;
  double command_{0.0};
};

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

---

四、CMakeLists.txt 与 package.xml 关键片段

CMakeLists.txt：

cmake

复制

cmake_minimum_required(VERSION 3.8)
project(pose_server_cpp)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)

# 生成接口
rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/SetCommandGetPose.srv"
  DEPENDENCIES geometry_msgs
)

# 服务端可执行文件
add_executable(pose_server src/pose_server.cpp)
ament_target_dependencies(pose_server rclcpp geometry_msgs)

rosidl_target_interfaces(pose_server ${PROJECT_NAME} "rosidl_typesupport_cpp")

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

ament_package()

package.xml（注意 group 标签）：

xml

复制

<depend>rclcpp</depend>
<depend>geometry_msgs</depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

---

五、编译 & 运行

bash

复制

cd ~/ws
colcon build --packages-select pose_server_cpp
source install/setup.bash

# 终端1：启动服务端
ros2 run pose_server_cpp pose_server

# 终端2：用命令行测试
ros2 service call /set_command_get_pose pose_server_cpp/srv/SetCommandGetPose "{command: 1.0}"

---

六、客户端（C++示例，可选）

cpp

复制

// src/pose_client.cpp
#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "pose_server_cpp/srv/set_command_get_pose.hpp"

using namespace std::chrono_literals;

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("pose_client");
  auto client = node->create_client<pose_server_cpp::srv::SetCommandGetPose>("set_command_get_pose");

  while (!client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for service.");
      return 0;
    }
    RCLCPP_INFO(node->get_logger(), "Service not available, waiting again...");
  }

  auto request = std::make_shared<pose_server_cpp::srv::SetCommandGetPose::Request>();
  request->command = 2.0;

  auto result = client->async_send_request(request);
  if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
    RCLCPP_INFO(node->get_logger(),
                "Got pose: x=%.2f", result.get()->pose.position.x);
  } else {
    RCLCPP_ERROR(node->get_logger(), "Failed to call service");
  }

  rclcpp::shutdown();
  return 0;
}

把 pose_client.cpp 也加到 CMakeLists.txt 的 add_executable 与 install(TARGETS …) 即可。

---

七、如何换成真实位姿

1. 在 PoseServer 中订阅 /odom 或 /amcl_pose 等话题，把收到的 Pose 存到成员变量。

2. 在 handle_request 时把最新收到的位姿直接赋给 response->pose 即可。

至此，一个完整的 ROS 2 C++ Service Server 已经搭建完毕，既能接收控制命令，又能实时返回机器人位置。