cimSLPService getSLPData(cimomConfig cfg, const CMPIBroker *_broker, const CMPIContext *ctx, const char *urlsyntax) { CMPIInstance **ci; //CMPIStatus status; //CMPIConstClass *ccls; cimSLPService rs; // service which is going to be returned // to the calling function char *sn; #ifdef SLP_HOSTNAME_LIB static void *hostnameLib = NULL; static getSlpHostname gethostname; char *ln; char dlName[512]; int err; err = 1; if (getControlChars("slpHostnamelib", &ln) == 0) { libraryName(NULL, ln, dlName, 512); if ((hostnameLib = dlopen(dlName, RTLD_LAZY))) { gethostname = dlsym(hostnameLib, "_sfcGetSlpHostname"); if (gethostname) err = 0; } } if (err) mlogf(M_ERROR, M_SHOW, "--- SLP Hostname exit %s not found. Defaulting to system hostname.\n", dlName); #endif _SFCB_ENTER(TRACE_SLP, "getSLPData"); memset(&rs, 0, sizeof(cimSLPService)); // first of all, get the interop namespace, needed for all further // connections // this call fills the array as well as sets the global interOpNS // variable rs.InteropSchemaNamespace = getInterOpNS(); // extract all relavant stuff for SLP out of CIM_ObjectManager // construct the server string ci = myGetInstances(_broker, ctx, interOpNS, "CIM_ObjectManager", urlsyntax); if (ci) { sn = myGetProperty(ci[0], "SystemName"); #ifdef SLP_HOSTNAME_LIB if (!err) { char *tmp; if ((err = gethostname(&tmp))) { free(sn); sn = tmp; } else { printf ("-#- SLP call to %s for hostname failed. Defaulting to system hostname.\n", dlName); } } #endif rs.url_syntax = getUrlSyntax(sn, cfg.commScheme, cfg.port); rs.service_hi_name = myGetProperty(ci[0], "ElementName"); rs.service_hi_description = myGetProperty(ci[0], "Description"); rs.service_id = myGetProperty(ci[0], "Name"); freeInstArr(ci); } // extract all relavant stuff for SLP out of // CIM_ObjectManagerCommunicationMechanism ci = myGetInstances(_broker, ctx, interOpNS, "CIM_ObjectManagerCommunicationMechanism", urlsyntax); if (ci) { rs.CommunicationMechanism = myGetProperty(ci[0], "CommunicationMechanism"); rs.OtherCommunicationMechanismDescription = myGetProperty(ci[0], "OtherCommunicationMechanism"); rs.ProtocolVersion = myGetProperty(ci[0], "Version"); rs.FunctionalProfilesSupported = myGetPropertyArray(ci[0], "FunctionalProfilesSupported"); rs.FunctionalProfileDescriptions = myGetPropertyArray(ci[0], "FunctionalProfileDescriptions"); rs.MultipleOperationsSupported = myGetProperty(ci[0], "MultipleOperationsSupported"); rs.AuthenticationMechanismsSupported = myGetPropertyArray(ci[0], "AuthenticationMechanismsSupported"); rs.AuthenticationMechansimDescriptions = myGetPropertyArray(ci[0], "AuthenticationMechansimDescriptions"); // do the transformations from numbers to text via the qualifiers //CMPIStatus myRc; //CMPIObjectPath *myOp = CMGetObjectPath(ci[0], &myRc); //CMPIData qd = CMGetPropertyQualifier(myOp, "CommunicationMechanism", "ValueMap", &myRc); rs.CommunicationMechanism = transformValue(rs.CommunicationMechanism, CMGetObjectPath(ci[0], NULL), "CommunicationMechanism"); rs.FunctionalProfilesSupported = transformValueArray(rs.FunctionalProfilesSupported, CMGetObjectPath(ci[0], NULL), "FunctionalProfilesSupported"); rs.AuthenticationMechanismsSupported = transformValueArray(rs.AuthenticationMechanismsSupported, CMGetObjectPath(ci[0], NULL), "AuthenticationMechanismsSupported"); freeInstArr(ci); } // extract all relavant stuff for SLP out of CIM_Namespace ci = myGetInstances(_broker, ctx, interOpNS, "CIM_Namespace", urlsyntax); if (ci) { rs.Namespace = myGetPropertyArrayFromArray(ci, "Name"); rs.Classinfo = myGetPropertyArrayFromArray(ci, "ClassInfo"); freeInstArr(ci); } // extract all relavant stuff for SLP out of CIM_RegisteredProfile //CMPIContext *ctxLocal = prepareUpcall(ctx); //ci = myGetInstances(_broker, ctxLocal, interOpNS, "CIM_RegisteredProfile", urlsyntax); ci = myGetInstances(_broker, ctx, interOpNS, "CIM_RegisteredProfile", urlsyntax); if (ci) { rs.RegisteredProfilesSupported = myGetRegProfiles(_broker, ci, ctx); //rs.RegisteredProfilesSupported = myGetRegProfiles(_broker, ci, ctxLocal); freeInstArr(ci); } //CMRelease(ctxLocal); _SFCB_RETURN(rs); }
std::shared_ptr<drake::lcmt_qp_controller_input> encodeQPInputLCM(const mxArray *qp_input) { // Take a matlab data structure corresponding to a QPInputConstantHeight object and parse it down to its representation as an equivalent LCM message. std::shared_ptr<drake::lcmt_qp_controller_input> msg (new drake::lcmt_qp_controller_input()); msg->timestamp = (int64_t) (mxGetScalar(myGetProperty(qp_input, "timestamp")) * 1000000); const mxArray* zmp_data = myGetProperty(qp_input, "zmp_data"); matlabToCArrayOfArrays<4, 4>(zmp_data, 0, "A", &msg->zmp_data.A[0][0]); matlabToCArrayOfArrays<4, 2>(zmp_data, 0, "B", &msg->zmp_data.B[0][0]); matlabToCArrayOfArrays<2, 4>(zmp_data, 0, "C", &msg->zmp_data.C[0][0]); matlabToCArrayOfArrays<2, 2>(zmp_data, 0, "D", &msg->zmp_data.D[0][0]); matlabToCArrayOfArrays<4, 1>(zmp_data, 0, "x0", &msg->zmp_data.x0[0][0]); matlabToCArrayOfArrays<2, 1>(zmp_data, 0, "y0", &msg->zmp_data.y0[0][0]); matlabToCArrayOfArrays<2, 1>(zmp_data, 0, "u0", &msg->zmp_data.u0[0][0]); matlabToCArrayOfArrays<2, 2>(zmp_data, 0, "R", &msg->zmp_data.R[0][0]); matlabToCArrayOfArrays<2, 2>(zmp_data, 0, "Qy", &msg->zmp_data.Qy[0][0]); matlabToCArrayOfArrays<4, 4>(zmp_data, 0, "S", &msg->zmp_data.S[0][0]); matlabToCArrayOfArrays<4, 1>(zmp_data, 0, "s1", &msg->zmp_data.s1[0][0]); matlabToCArrayOfArrays<4, 1>(zmp_data, 0, "s1dot", &msg->zmp_data.s1dot[0][0]); msg->zmp_data.s2 = mxGetScalar(myGetField(zmp_data, "s2")); msg->zmp_data.s2dot = mxGetScalar(myGetField(zmp_data, "s2dot")); msg->zmp_data.timestamp = msg->timestamp; const mxArray* support_data = myGetProperty(qp_input, "support_data"); int nsupp = mxGetN(support_data); msg->num_support_data = (int32_t) nsupp; double double_logic_map[4][1]; msg->support_data.resize(nsupp); if (nsupp > 0) { if (mxGetM(support_data) != 1) { mexErrMsgTxt("support data should be a struct array with M=1"); } for (int i=0; i < nsupp; i++) { msg->support_data[i].timestamp = msg->timestamp; msg->support_data[i].body_id = (int32_t) mxGetScalar(myGetField(support_data, i, "body_id")); const mxArray *contact_pts = myGetField(support_data, i, "contact_pts"); if (!contact_pts) mexErrMsgTxt("couldn't get points"); Map<MatrixXd>contact_pts_mat(mxGetPr(contact_pts), mxGetM(contact_pts), mxGetN(contact_pts)); msg->support_data[i].num_contact_pts = (int32_t) mxGetN(contact_pts); msg->support_data[i].contact_pts.resize(3); for (int j=0; j < 3; j++) { msg->support_data[i].contact_pts[j].resize(msg->support_data[i].num_contact_pts); for (int k=0; k < msg->support_data[i].num_contact_pts; k++) { msg->support_data[i].contact_pts[j][k] = contact_pts_mat(j, k); } } matlabToCArrayOfArrays<4, 1>(support_data, i, "support_logic_map", &double_logic_map[0][0]); for (int j=0; j < 4; j++) { msg->support_data[i].support_logic_map[j] = (double_logic_map[j][0] != 0); } msg->support_data[i].mu = mxGetScalar(myGetField(support_data, i, "mu")); msg->support_data[i].contact_surfaces = (int32_t) mxGetScalar(myGetField(support_data, i, "contact_surfaces")); } } const mxArray* body_motion_data = myGetProperty(qp_input, "body_motion_data"); const int nbod = mxGetN(body_motion_data); msg->num_tracked_bodies = nbod; msg->body_motion_data.resize(nbod); if (nbod > 0) { if (mxGetM(body_motion_data) != 1) { mexErrMsgTxt("body motion data should be a 1xN struct array"); } for (int i=0; i < nbod; i++) { msg->body_motion_data[i].timestamp = msg->timestamp; msg->body_motion_data[i].body_id = (int32_t) mxGetScalar(myGetField(body_motion_data, i, "body_id")); memcpy(msg->body_motion_data[i].ts, mxGetPr(myGetField(body_motion_data, i, "ts")), 2*sizeof(double)); const mxArray* coefs = myGetField(body_motion_data, i, "coefs"); if (mxGetNumberOfDimensions(coefs) != 3) mexErrMsgTxt("coefs should be a dimension-3 array"); const mwSize* dim = mxGetDimensions(coefs); if (dim[0] != 6 || dim[1] != 1 || dim[2] != 4) mexErrMsgTxt("coefs should be size 6x4"); matlabToCArrayOfArrays<6, 4>(body_motion_data, i, "coefs", &msg->body_motion_data[i].coefs[0][0]); } } const mxArray* whole_body_data = myGetProperty(qp_input, "whole_body_data"); if (mxGetN(whole_body_data) != 1 || mxGetM(whole_body_data) != 1) mexErrMsgTxt("whole_body_data should be a 1x1 struct"); const mxArray* q_des = myGetField(whole_body_data, "q_des"); if (mxGetN(q_des) != 1) mexErrMsgTxt("q_des should be a column vector"); const int npos = mxGetM(q_des); msg->whole_body_data.timestamp = msg->timestamp; msg->whole_body_data.num_positions = npos; Map<VectorXd>q_des_vec(mxGetPr(q_des), npos); msg->whole_body_data.q_des.resize(npos); for (int i=0; i < npos; i++) { msg->whole_body_data.q_des[i] = q_des_vec(i); } const mxArray* condof = myGetField(whole_body_data, "constrained_dofs"); const int ncons = mxGetM(condof); msg->whole_body_data.num_constrained_dofs = ncons; msg->whole_body_data.constrained_dofs.resize(ncons); if (ncons > 0) { if (mxGetN(condof) != 1) mexErrMsgTxt("constrained dofs should be a column vector"); Map<VectorXd>condof_vec(mxGetPr(condof), ncons); for (int i=0; i < ncons; i++) { msg->whole_body_data.constrained_dofs[i] = condof_vec(i); } } msg->param_set_name = mxArrayToString(myGetProperty(qp_input, "param_set_name")); return msg; }