ROS服务客户端array_q为空，服务端已初始化，请求排查原因

问题描述 #

ROS Service Client中array_q数组为空，但Server端已通过打印验证该数组已正确初始化。以下是服务端和客户端C++代码，请求排查问题原因：

服务端代码 #

bool inverse(motion_planner::InverseKinematic::Request &req, motion_planner::InverseKinematic::Response &res){
    double scaleFactor = 1.0;
    double Tf = 10.0;
    double DeltaT = 0.5;
    Eigen::VectorXd T;
    T = Eigen::VectorXd::LinSpaced(static_cast<int>((Tf / DeltaT) + 1), 0, Tf);

    Eigen::VectorXd jointstate = Eigen::Map<Eigen::VectorXd>(req.jointstate.data(), req.jointstate.size());

    auto result = CinematicaDiretta(jointstate, scaleFactor);   
    Eigen::VectorXd xe = result.pe;     
    Eigen::Matrix3d Re = result.Re;    
    Eigen::Quaterniond q0(Re);          

    ROS_INFO("--DERIVE INITIAL INFORMATION xe, Re, RPY, q0 of END EFFECTOR------");
    ROS_INFO("Vector xe: %s", vectorToString(xe).c_str());
    ROS_INFO("Matrix Re:%s", matrix3dToString(Re).c_str());
    ROS_INFO("Vector RPY: %s ", vectorToString(Re.eulerAngles(0, 1, 2)).c_str());
    ROS_INFO("Quaternion q0: %s \n", quaternionToString(q0).c_str());

    ROS_INFO("--REQUEST DESIRED END EFFECTOR ----------");
    ROS_INFO("Vector Location xef : %f, %f, %f", req.xef[0], req.xef[1], req.xef[2]);
    ROS_INFO("Vector Euler phief : %f, %f, %f", req.phief[0], req.phief[1], req.phief[2]);

    Eigen::VectorXd phief = Eigen::Map<Eigen::VectorXd>(req.phief.data(), req.phief.size());
    Eigen::VectorXd xef = Eigen::Map<Eigen::VectorXd>(req.xef.data(), req.xef.size());

    Eigen::Matrix3d Ref;
    Ref = euler2RotationMatrix(phief, "XYZ");
    ROS_INFO("Matrix Ref:%s ", matrix3dToString(Ref).c_str());

    Eigen::Matrix4d Tt0 = Eigen::Matrix4d::Identity();
    Tt0.block<3, 3>(0, 0) = Ref;
    Tt0.block<3, 1>(0, 3) = xef;
    Eigen::Quaterniond qf(Tt0.block<3, 3>(0, 0));
    ROS_INFO("Quaternion qf: %s \n", quaternionToString(qf).c_str());

    Eigen::Matrix3d Kp = 10.0 * Eigen::Matrix3d::Identity();
    Eigen::Matrix3d Kq = -10.0 * Eigen::Matrix3d::Identity();

    Eigen::MatrixXd Th = invDiffKinematicControlSimCompleteQuaternion(jointstate, Kp, Kq, T, 0.0, Tf, DeltaT, scaleFactor, Tf, xe, xef, q0, qf);
    ROS_INFO("--DERIVED q ------");
    ROS_INFO("Dimensioni di Th: %ld %ld", Th.rows(), Th.cols());
    ROS_INFO("%s", matrixToString(Th).c_str());


    for (int i = 0; i < Th.rows(); i++) { 
        for (int j = 0; j < Th.cols(); j++) {
            //res.array_q.push_back(Th(i,j));
            if (i >= 0 && i < Th.rows() && j >= 0 && j < Th.cols()) {
                res.array_q.push_back(Th(i, j));
            } else {
                ROS_ERROR("Indici fuori dai limiti: i=%d, j=%d", i, j);
            }
        }
    }

    for(int i=0; i < res.array_q.size(); i++){
        std::cout << res.array_q[i];
    }
    std::cout<<"\n";

    ROS_INFO("-- END REQUEST ------");

    
    return true;
}


int main(int argc, char **argv){

    ros::init(argc, argv, "inverse_kinemtic_node");
    ros::NodeHandle n;
    ros::ServiceServer service = n.advertiseService("calculate_inverse_kinematics", inverse);

    ros::spin();
     
    return 0;
} 

客户端代码 #

Eigen::MatrixXd ask_inverse_kinematic(ros::NodeHandle& n, double xef[3], double phief[3]){
 
    
    ros::ServiceClient service_client = n.serviceClient<motion_planner::InverseKinematic>("calculate_inverse_kinematics");
   
    std::vector<double> received_positions;
    ros::Subscriber sub1 = n.subscribe<sensor_msgs::JointState>("/ur5/joint_states", 1, std::bind(callback, std::placeholders::_1, &received_positions));

  
    while (received_positions.empty()) {
        ros::spinOnce(); 
    }

    motion_planner::InverseKinematic srv;  

    srv.request.jointstate.push_back(received_positions[4]);
    srv.request.jointstate.push_back(received_positions[3]);
    srv.request.jointstate.push_back(received_positions[0]);
    srv.request.jointstate.push_back(received_positions[5]);
    srv.request.jointstate.push_back(received_positions[6]);
    srv.request.jointstate.push_back(received_positions[7]);

    srv.request.xef[0]=xef[0];          srv.request.phief[0]=phief[0];
    srv.request.xef[1]=xef[1];          srv.request.phief[1]=phief[1];
    srv.request.xef[2]=xef[2];          srv.request.phief[2]=phief[2];

    std::cout << "RISPOSTA: " << srv.response.array_q.size();

    Eigen::MatrixXd ret(srv.response.array_q.size() / 6 ,6);
    ROS_INFO("Dimensioni di Th: %ld %ld", ret.rows(), ret.cols());
    
    if (service_client.call(srv)){
        std::stringstream q_received;
        for(int i=0; i < srv.response.array_q.size() / 6; i++){
            for(int j=0; j<6; j++){
                ret(i,j) = srv.response.array_q[i+j];
                q_received << srv.response.array_q[i] << "  ";
            }  
            q_received << "\n";
        }
        std::printf("Joint State da Raggiungere: %s \n", q_received.str().c_str());
    } else {
        std::cout << "Failed to call service 'calculate_inverse_kinematics' \n";
    }

    return ret;
  
}

问题排查关键点 #

• 提前访问响应数据：客户端在调用service_client.call(srv)之前就打印srv.response.array_q.size()并创建返回矩阵ret，此时服务未调用，响应必然为空，这是导致你误以为响应为空的直接原因。应将响应数据的访问逻辑移到call()成功之后。
• 响应数据索引错误：客户端中ret(i,j) = srv.response.array_q[i+j];的索引计算错误。服务端是按行遍历矩阵Th，将元素依次加入array_q，因此第i行第j列的元素位置应为i*6 + j，而非i+j。错误的索引会导致读取错误数据，甚至在数组长度不足时出现未定义行为。
• 订阅者生命周期问题：客户端中ros::Subscriber sub1是局部变量，在while循环结束后可能被销毁。虽然此处spinOnce()已获取到数据，但建议将订阅者声明在更大的作用域内，避免潜在的生命周期问题。
• 响应数据有效性检查：在服务调用成功后，先打印srv.response.array_q.size()确认数据是否真的为空，再进行后续的矩阵转换操作，避免基于空数据进行计算。

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