int main() { lcm_t * lcm = lcm_create(NULL); BotParam * param = bot_param_new_from_server(lcm, 1); if (param == NULL) { fprintf(stderr, "could not get params!\n"); exit(1); } bot_param_add_update_subscriber(param,param_update_handler,lcm); char * s; int ret = bot_param_write_to_string(param, &s); fprintf(stderr, "%s", s); free(s); // double foo = bot_param_get_double_or_fail(param, "foo"); // double bar = bot_param_get_double_or_fail(param, "bar"); // printf("foo=%f, bar = %f\n", foo, bar); bot_param_write(param, stderr); while (1) { lcm_handle(lcm); char * key = "coordinate_frames.body.history"; fprintf(stderr, "%s = %d\n", key, bot_param_get_int_or_fail(param, key)); } return 0; }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { if ((nrhs == 0) || !mxIsChar(prhs[0])) { mexErrMsgTxt("BotParamClient: first argument must be command string"); } if ((nrhs < 2) || !mxIsChar(prhs[1])) { mexErrMsgTxt("BotParamClient: second argument must be key"); } std::string command = ::getString(prhs[0]); std::transform(command.begin(), command.end(), command.begin(), ::tolower); std::string key = ::getString(prhs[1]); if ((nrhs > 3) || ((nrhs == 3) && !::isSetCommand(command))) { mexErrMsgTxt("BotParamClient: too many input arguments"); } if (isSetCommand(command) && (nrhs != 3)) { mexErrMsgTxt("BotParamClient: need value argument"); } BotParam* param = BotParamClient::instance().getUnderlyingBotParam(); if (param == NULL) { mexErrMsgTxt("BotParamClient: no param client; is server running?"); } bool hasKey = (0 != bot_param_has_key(param, key.c_str())); if (!hasKey && !isSetCommand(command) && (command != "haskey")) { mexErrMsgTxt("BotParamClient: invalid key"); } if (command == "haskey") { plhs[0] = mxCreateLogicalMatrix(1,1); mxLogical* out = mxGetLogicals(plhs[0]); out[0] = hasKey; } else if (command == "subkeys") { char** subkeysRaw = bot_param_get_subkeys(param, key.c_str()); std::vector<std::string> subkeys; for (char** subkeyPtr = subkeysRaw; *subkeyPtr != NULL; ++subkeyPtr) { subkeys.push_back(std::string(*subkeyPtr)); } bot_param_str_array_free(subkeysRaw); plhs[0] = mxCreateCellMatrix(1,subkeys.size()); for (size_t i = 0; i < subkeys.size(); ++i) { mxSetCell(plhs[0], i, mxCreateString(subkeys[i].c_str())); } } else if (command == "getnum") { int len = bot_param_get_array_len(param, key.c_str()); std::vector<double> vals; if (len <= 0) { double val; if (bot_param_get_double(param, key.c_str(), &val) != 0) { mexErrMsgTxt("BotParamClient: cannot find numeric"); } vals.push_back(val); } else { vals.resize(len); if (bot_param_get_double_array(param, key.c_str(), vals.data(), len) != len) { mexErrMsgTxt("BotParamClient: non-numeric value(s)"); } } plhs[0] = mxCreateDoubleMatrix(1,vals.size(),mxREAL); double* ptr = mxGetPr(plhs[0]); for (size_t i = 0; i < vals.size(); ++i) { ptr[i] = vals[i]; } } else if (command == "getbool") { int len = bot_param_get_array_len(param, key.c_str()); std::vector<bool> vals; if (len <= 0) { int val; if (bot_param_get_boolean(param, key.c_str(), &val) != 0) { mexErrMsgTxt("BotParamClient: cannot find bool"); } vals.push_back(val!=0); } else { int valsRaw[len]; if (bot_param_get_boolean_array(param, key.c_str(), valsRaw, len) != len) { mexErrMsgTxt("BotParamClient: non-boolean value(s)"); } for (int i = 0; i < len; ++i) { vals.push_back(valsRaw[i]!=0); } } plhs[0] = mxCreateLogicalMatrix(1,vals.size()); mxLogical* out = mxGetLogicals(plhs[0]); for (size_t i = 0; i < vals.size(); ++i) { out[i] = vals[i]; } } else if (command == "getstr") { int len = bot_param_get_array_len(param, key.c_str()); std::vector<std::string> vals; if (len <= 0) { char* val = NULL; if (bot_param_get_str(param, key.c_str(), &val) != 0) { mexErrMsgTxt("BotParamClient: cannot find string"); } vals.push_back(std::string(val)); free(val); } else { char** valsRaw = bot_param_get_str_array_alloc(param, key.c_str()); for (char** valsPtr = valsRaw; *valsPtr != NULL; ++valsPtr) { vals.push_back(std::string(*valsPtr)); } bot_param_str_array_free(valsRaw); } plhs[0] = mxCreateCellMatrix(1,vals.size()); for (size_t i = 0; i < vals.size(); ++i) { mxSetCell(plhs[0], i, mxCreateString(vals[i].c_str())); } } else if (command == "setstr") { bot_param::set_t msg = constructSetMessage(param, key); std::string value; if (mxIsCell(prhs[2])) { int len = mxGetNumberOfElements(prhs[2]); for (int i = 0; i < len; ++i) { mxArray* cellVal = mxGetCell(prhs[2],i); if (!mxIsChar(cellVal)) { mexErrMsgTxt("BotParamClient: invalid string value argument"); } std::string curVal = ::getString(cellVal); value += (curVal + std::string(",")); } if (value.size() > 0) { value = value.substr(0,value.size()-1); } msg.entries[0].is_array = true; } else { if (!mxIsChar(prhs[2])) { mexErrMsgTxt("BotParamClient: invalid string value argument"); } value = ::getString(prhs[2]); } msg.entries[0].value = value; BotParamClient::instance().getLcm()->publish("PARAM_SET", &msg); } else if (command == "setnum") { if (!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2])) { mexErrMsgTxt("BotParamClient: third argument must be real value array"); } bot_param::set_t msg = constructSetMessage(param, key); std::string value; int len = mxGetNumberOfElements(prhs[2]); double* valArray = mxGetPr(prhs[2]); for (int i = 0; i < len; ++i) { std::ostringstream oss; oss << valArray[i] << ","; value += oss.str(); } if (value.size() > 0) { value = value.substr(0,value.size()-1); } msg.entries[0].value = value; msg.entries[0].is_array = (len > 1); BotParamClient::instance().getLcm()->publish("PARAM_SET", &msg); } else if (command == "print") { bot_param_write(param, stderr); } else { mexErrMsgTxt("BotParamClient: command must be haskey, subkeys, getnum, getbool, getstr, or setstr"); } }