ROS 服务响应返回空数组

Question

在我的 Ros 服务客户端中，数组 array_q 为空，但在服务器端它已正确初始化。我确信初始化正确，感谢一些验证它的打印。

服务器端

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;
  
}

我不知道为什么会出现这个错误。请帮助我

Answer 1

服务器端

inverse

函数应该返回数组，而不是返回布尔值。