ROS C++ 实现消息通信与服务通信

ROS C++ 实现消息通信与服务通信

下面我将使用 C++ 实现完整的 ROS 消息通信和服务通信功能，包括自定义消息和服务。

完整实现方案

1. 创建功能包和自定义消息/服务

# 创建工作空间
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin_make

# 创建功能包
cd src
catkin_create_pkg msg_cpp_demo roscpp std_msgs message_generation message_runtime

# 创建消息和服务目录
cd msg_cpp_demo
mkdir msg srv src

# 创建自定义消息
echo -e "string content\nint32 id" > msg/DemoMsg.msg

# 创建自定义服务
echo -e "string request\n---\nstring response" > srv/DemoSrv.srv

2. 修改配置文件

package.xml:

<?xml version="1.0"?>
<package format="2">
  <name>msg_cpp_demo</name>
  <version>0.0.0</version>
  <description>ROS C++ Message and Service Demo</description>
  
  <maintainer email="user@example.com">User</maintainer>
  <license>MIT</license>

  <buildtool_depend>catkin</buildtool_depend>
  
  <depend>roscpp</depend>
  <depend>std_msgs</depend>
  
  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>
</package>

CMakeLists.txt:

cmake_minimum_required(VERSION 3.0.2)
project(msg_cpp_demo)

# 查找依赖
find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  message_generation
)

# 声明消息和服务
add_message_files(FILES DemoMsg.msg)
add_service_files(FILES DemoSrv.srv)

# 生成消息
generate_messages(DEPENDENCIES std_msgs)

# 配置 catkin 包
catkin_package(
  CATKIN_DEPENDS message_runtime roscpp std_msgs
)

# 包含目录
include_directories(
  ${catkin_INCLUDE_DIRS}
)

# 添加可执行文件
add_executable(publisher src/publisher.cpp)
target_link_libraries(publisher ${catkin_LIBRARIES})
add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp)

add_executable(subscriber src/subscriber.cpp)
target_link_libraries(subscriber ${catkin_LIBRARIES})
add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp)

add_executable(server src/server.cpp)
target_link_libraries(server ${catkin_LIBRARIES})
add_dependencies(server ${PROJECT_NAME}_generate_messages_cpp)

add_executable(client src/client.cpp)
target_link_libraries(client ${catkin_LIBRARIES})
add_dependencies(client ${PROJECT_NAME}_generate_messages_cpp)

3. 编写 C++ 节点代码

src/publisher.cpp - 消息发布者:

#include <ros/ros.h>
#include "msg_cpp_demo/DemoMsg.h"

int main(int argc, char **argv) {
    // 初始化节点
    ros::init(argc, argv, "publisher_node");
    ros::NodeHandle nh;
    
    // 创建发布者
    ros::Publisher pub = nh.advertise<msg_cpp_demo::DemoMsg>("demo_topic", 10);
    
    // 设置发布频率 (1Hz)
    ros::Rate rate(1);
    
    int msg_id = 0;
    
    ROS_INFO("Publisher started. Press Ctrl+C to exit.");
    
    while (ros::ok()) {
        // 创建消息
        msg_cpp_demo::DemoMsg msg;
        msg.content = "Hello ROS C++!";
        msg.id = msg_id++;
        
        // 发布消息
        pub.publish(msg);
        
        // 打印日志
        ROS_INFO("Published message: %s (ID: %d)", 
                 msg.content.c_str(), msg.id);
        
        // 休眠
        rate.sleep();
    }
    
    return 0;
}

src/subscriber.cpp - 消息订阅者:

#include <ros/ros.h>
#include "msg_cpp_demo/DemoMsg.h"

// 消息回调函数
void messageCallback(const msg_cpp_demo::DemoMsg::ConstPtr& msg) {
    ROS_INFO("Received message: %s (ID: %d)", 
             msg->content.c_str(), msg->id);
}

int main(int argc, char **argv) {
    // 初始化节点
    ros::init(argc, argv, "subscriber_node");
    ros::NodeHandle nh;
    
    // 创建订阅者
    ros::Subscriber sub = nh.subscribe("demo_topic", 10, messageCallback);
    
    ROS_INFO("Subscriber started. Waiting for messages...");
    
    // 进入循环处理回调
    ros::spin();
    
    return 0;
}

src/server.cpp - 服务端:

#include <ros/ros.h>
#include "msg_cpp_demo/DemoSrv.h"

// 服务处理函数
bool handleServiceRequest(
    msg_cpp_demo::DemoSrv::Request &req,
    msg_cpp_demo::DemoSrv::Response &res
) {
    ROS_INFO("Received service request: %s", req.request.c_str());
    
    // 处理请求
    res.response = "Processed: " + req.request;
    
    ROS_INFO("Sending response: %s", res.response.c_str());
    return true;
}

int main(int argc, char **argv) {
    // 初始化节点
    ros::init(argc, argv, "service_server");
    ros::NodeHandle nh;
    
    // 创建服务
    ros::ServiceServer service = nh.advertiseService(
        "demo_service", handleServiceRequest
    );
    
    ROS_INFO("Service server ready. Waiting for requests...");
    
    // 进入循环处理请求
    ros::spin();
    
    return 0;
}

src/client.cpp - 客户端:

#include <ros/ros.h>
#include "msg_cpp_demo/DemoSrv.h"

int main(int argc, char **argv) {
    // 初始化节点
    ros::init(argc, argv, "service_client");
    ros::NodeHandle nh;
    
    // 等待服务可用
    ROS_INFO("Waiting for service to become available...");
    ros::service::waitForService("demo_service");
    
    // 创建服务客户端
    ros::ServiceClient client = nh.serviceClient<msg_cpp_demo::DemoSrv>("demo_service");
    
    // 准备服务请求
    msg_cpp_demo::DemoSrv srv;
    srv.request.request = "Test Request from C++ Client";
    
    // 调用服务
    if (client.call(srv)) {
        ROS_INFO("Service call successful. Response: %s", 
                 srv.response.response.c_str());
    } else {
        ROS_ERROR("Failed to call service demo_service");
        return 1;
    }
    
    return 0;
}

4. 编译与运行

# 编译工作空间
cd ~/catkin_ws
catkin_make

# 加载环境
source devel/setup.bash

测试消息通信

# 终端1: 运行发布者
rosrun msg_cpp_demo publisher

# 终端2: 运行订阅者
rosrun msg_cpp_demo subscriber

测试服务通信

# 终端1: 运行服务端
rosrun msg_cpp_demo server

# 终端2: 运行客户端
rosrun msg_cpp_demo client

5. 预期输出

消息通信测试:

• 发布者终端每秒输出: Published message: Hello ROS C++! (ID: [递增数字])
• 订阅者终端实时输出: Received message: Hello ROS C++! (ID: [相同数字])

服务通信测试:

• 服务端输出:

  Service server ready. Waiting for requests...
  Received service request: Test Request from C++ Client
  Sending response: Processed: Test Request from C++ Client
  

• 客户端输出:

  Waiting for service to become available...
  Service call successful. Response: Processed: Test Request from C++ Client
  

关键点说明

1. 自定义消息/服务生成:

   • ROS 会在编译时自动生成 C++ 头文件
   • 消息头文件路径: devel/include/msg_cpp_demo/DemoMsg.h
   • 服务头文件路径: devel/include/msg_cpp_demo/DemoSrv.h

2. 节点依赖关系:

   • 所有节点都依赖于生成的消息头文件
   • 使用 add_dependencies() 确保消息在节点编译前生成

3. 服务调用模式:

   • 客户端使用 waitForService() 确保服务可用
   • 服务调用是同步阻塞操作
   • 服务端返回 true 表示成功处理请求

4. 消息发布/订阅:

   • 发布者和订阅者通过话题名连接
   • 使用回调函数处理接收到的消息
   • ros::spin() 用于处理回调

这个实现完整展示了 ROS C++ 中的消息通信和服务通信机制，包括自定义消息和服务的创建与使用。