基于自定义srv的ROS IMU服务服务端与客户端节点模板开发咨询

功能背景 #

基于ESC32微控制器做服务端采集IMU原始数据，接收请求后将数据回传给树莓派4B客户端做后续计算，仅传输原始IMU数值。 #

1. 自定义服务接口定义 #

srv/ImuValue.srv 文件内容如下：

float64 x_orient_in
float64 y_orient_in
float64 z_orient_in
float64 w_orient_in
float64 x_veloc_in
float64 y_veloc_in
float64 z_veloc_in
float64 x_accel_in
float64 y_accel_in
float64 z_accel_in
---
float64 x_orient_out
float64 y_orient_out
float64 z_orient_out
float64 w_orient_out
float64 x_veloc_out
float64 y_veloc_out
float64 z_veloc_out
float64 x_accel_out
float64 y_accel_out
float64 z_accel_out
bool success

注意：实际使用时如果不需要请求端传参数，可以删掉---上方的所有请求字段，服务端直接在响应里填IMU数据返回即可，不需要接收请求参数 #

2. 服务端（ESC32端）CPP代码 #

原代码存在三个问题：1. 线性加速度字段重复赋值，y、z轴加速度未正确赋值；2. 服务回调函数未返回布尔值；3. 逻辑为直接回显请求参数，未实际读取本地IMU数据填入响应。修正后代码如下：

#include "ros/ros.h"
#include <sensor_msgs/Imu.h>
#include "ros_services/ImuValue.h"

// 全局变量存储最新IMU数据，也可以用类成员变量更规范
sensor_msgs::Imu latest_imu_data;

// IMU话题订阅回调，更新最新IMU数据
void imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
    latest_imu_data = *msg;
}

bool get_val(ros_services::ImuValue::Request  &req, ros_services::ImuValue::Response &res)
{
    ROS_INFO("收到IMU数据请求，正在返回响应");
    // 把本地最新的IMU数据填入响应字段
    res.x_orient_out = latest_imu_data.orientation.x;
    res.y_orient_out = latest_imu_data.orientation.y;
    res.z_orient_out = latest_imu_data.orientation.z;
    res.w_orient_out = latest_imu_data.orientation.w;
    res.x_veloc_out = latest_imu_data.angular_velocity.x;
    res.y_veloc_out = latest_imu_data.angular_velocity.y;
    res.z_veloc_out = latest_imu_data.angular_velocity.z;
    res.x_accel_out = latest_imu_data.linear_acceleration.x;
    res.y_accel_out = latest_imu_data.linear_acceleration.y;
    res.z_accel_out = latest_imu_data.linear_acceleration.z;
    res.success = true;
    return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "imu_status_server");
  ros::NodeHandle n;
  // 订阅本地IMU话题获取实时数据
  ros::Subscriber imu_sub = n.subscribe<sensor_msgs::Imu>("/thrbot/imu", 10, imu_callback);
  // 注册IMU查询服务
  ros::ServiceServer service = n.advertiseService("get_imu_data", get_val);
  ROS_INFO("IMU数据服务已启动");
  ros::spin();
  return 0;
}

3. 客户端（树莓派端）CPP代码 #

#include "ros/ros.h"
#include "ros_services/ImuValue.h"

int main(int argc, char **argv)
{
  ros::init(argc, argv, "imu_client_node");
  ros::NodeHandle n;
  // 创建服务客户端，连接服务端的get_imu_data服务
  ros::ServiceClient client = n.serviceClient<ros_services::ImuValue>("get_imu_data");
  ros_services::ImuValue srv;
  // 如果你的服务接口删掉了请求字段，这里不需要给srv.request赋值
  // 发起服务调用
  if (client.call(srv))
  {
    if(srv.response.success)
    {
      ROS_INFO("获取IMU数据成功:");
      ROS_INFO("四元数: x=%.6f, y=%.6f, z=%.6f, w=%.6f", 
                srv.response.x_orient_out, srv.response.y_orient_out, 
                srv.response.z_orient_out, srv.response.w_orient_out);
      ROS_INFO("角速度: x=%.6f, y=%.6f, z=%.6f", 
                srv.response.x_veloc_out, srv.response.y_veloc_out, srv.response.z_veloc_out);
      ROS_INFO("线加速度: x=%.6f, y=%.6f, z=%.6f", 
                srv.response.x_accel_out, srv.response.y_accel_out, srv.response.z_accel_out);
    }
    else
    {
      ROS_ERROR("服务端返回数据失败");
    }
  }
  else
  {
    ROS_ERROR("调用IMU服务失败，请检查服务端是否正常运行");
    return 1;
  }
  return 0;
}

4. 所用IMU话题原始数据格式 #

header: 
  seq: 45672
  stamp: 
    secs: 956
    nsecs: 962000000
  frame_id: "thrbot/imu_link"
orientation: 
  x: 0.0697171053094
  y: 0.00825242210747
  z: 0.920964387685
  w: -0.383270164991
orientation_covariance: [0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001]
angular_velocity: 
  x: 0.00156996015527
  y: 0.0263644782572
  z: -0.0617661883137
angular_velocity_covariance: [1.1519236000000001e-07, 0.0, 0.0, 0.0, 1.1519236000000001e-07, 0.0, 0.0, 0.0, 1.1519236000000001e-07]
linear_acceleration: 
  x: 1.32039048367
  y: -0.376341478938
  z: 9.70643773249
linear_acceleration_covariance: [1.6e-05, 0.0, 0.0, 0.0, 1.6e-05, 0.0, 0.0, 0.0, 1.6e-05]

内容的提问来源于stack exchange，提问作者Bob9710