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 COMPONENTSroscppstd_msgsmessage_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++ 中的消息通信和服务通信机制，包括自定义消息和服务的创建与使用。