如何在Gazebo仿真中控制UUV时按下摇杆通过launch文件调用ROS服务

现有代码已知问题梳理 #

• 服务端的服务回调函数没有返回布尔值，ROS要求服务处理函数必须返回true表示处理完成
• 客户端srv包名与服务端不匹配：服务端用的是imu_service/ImuValue，客户端错误引用了ros_services/ImuValue，会导致服务调用失败
• 客户端主函数逻辑错误：初始化后连续调用两次服务，且没有加ros::spin()，节点初始化后会直接退出，无法持续响应摇杆回调
• 控制器没有接入IMU反馈：当前客户端只是打印IMU数值，没有将获取到的IMU数据传入UUV的PID控制器作为反馈输入
• launch文件中没有加入IMU服务端和客户端节点，启动控制脚本时两个自定义节点不会运行

修正后的代码实现 #

1. 服务端代码修正 #

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

ros::ServiceServer service;
double get_imu_orientation_x;
// 后续可添加其他IMU字段：角速度、线加速度等

bool get_val(imu_service::ImuValue::Request  &req, imu_service::ImuValue::Response &res)
{
    ROS_INFO("sending back IMU response");    
    res.current_x_orientation_s = get_imu_orientation_x;
    res.success = true;
    // 填充其他IMU数值
    return true; // 必须返回true表示服务处理完成
}

void imuCallback(const  sensor_msgs::ImuConstPtr& msg)
{
     get_imu_orientation_x = msg->orientation.x;
     // 同步其他IMU字段存储
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "imu_status_server");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/thrbot/imu", 10, imuCallback);
  service = n.advertiseService("imu_status_server", get_val);
  ROS_INFO("IMU status server started...");
  ros::spin();
  return 0;
}

2. 客户端代码修正 #

#include "ros/ros.h"
// 修正srv包名和服务端一致
#include "imu_service/ImuValue.h"
#include <cstdlib>
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>

ros::ServiceClient client;
// 如果要把IMU数据传给控制器，可定义发布器发布到控制器的反馈输入话题
ros::Publisher imu_feedback_pub;

void joystick_callback(const sensor_msgs::Joy::ConstPtr& joy)
{
  // 0对应Xbox有线手柄A按键，可根据需求改为摇杆触发，比如只要摇杆有输入就调用服务
  auto button_pressed = joy->buttons[0];
  // 如需摇杆移动时自动触发，可替换判断条件为：joy->axes[0] !=0 || joy->axes[1] !=0 等
  if(button_pressed){
    imu_service::ImuValue srv;
    if(client.call(srv)){
      ROS_INFO("Got IMU x orientation: %f", srv.response.current_x_orientation_s);
      // 此处添加控制器反馈逻辑，将IMU数据组装后发布到控制器对应的反馈话题
      // 示例：geometry_msgs::Twist feedback_msg;
      // feedback_msg.angular.x = srv.response.current_x_orientation_s;
      // imu_feedback_pub.publish(feedback_msg);
    }else{
      ROS_ERROR("Failed to call IMU status service");
    }
  }
}


int main(int argc, char **argv)
{
  ros::init(argc,argv,"imu_client_node");
  ros::NodeHandle n;
  ros::NodeHandle nh_;
  ros::Subscriber joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, joystick_callback);
  // 初始化服务客户端
  client = n.serviceClient<imu_service::ImuValue>("imu_status_server");
  // 初始化控制器反馈发布器，话题名根据实际控制器输入修改
  // imu_feedback_pub = nh_.advertise<geometry_msgs::Twist>("/thrbot/imu_feedback", 10);
  // 等待服务上线避免第一次调用失败
  client.waitForExistence();
  ROS_INFO("IMU client node started, waiting for joystick input...");
  // 加入spin保持节点运行，持续响应回调
  ros::spin();
  return 0;
}

3. srv文件无需修改 #

确保文件存放在imu_service包的srv目录下，且包的CMakeLists.txt已经正确配置srv编译规则即可。

4. 控制launch文件修改 #

在launch文件末尾</launch>之前添加两个节点的启动配置：

<!-- 启动IMU服务端节点 -->
<node name="imu_status_server" pkg="imu_service" type="imu_status_server" output="screen"/>
<!-- 启动IMU客户端节点 -->
<node name="imu_client_node" pkg="imu_service" type="imu_client_node" output="screen"/>

注意将pkg的值改为你实际存放两个节点的功能包名。

功能验证步骤 #

1. 重新编译工作空间：catkin_make 或者 catkin build
2. 启动修改后的控制launch文件
3. 按下手柄A按键（或触发你设置的摇杆触发条件），查看终端是否打印IMU数值，确认服务调用正常
4. 按照你使用的PID控制器的反馈输入要求，完善客户端的反馈发布逻辑即可实现IMU数据作为控制反馈的需求

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