LCMFrontEnd::LCMFrontEnd(const std::string & in_log_fname, const std::string & out_log_fname, const std::string & param_fname, const std::string & param_override_str, const std::string & begin_timestamp, double processing_rate) { state_estimator = NULL; bool running_from_log = !in_log_fname.empty(); bool running_to_log = !out_log_fname.empty(); if (running_from_log && in_log_fname == out_log_fname) { fprintf(stderr, "must specify different logname for output %s!\n", out_log_fname.c_str()); exit(1); } if (running_from_log) { printf("running from log file: %s\n", in_log_fname.c_str()); //std::string lcmurl = "file://" + in_log_fname + "?speed=0"; std::stringstream lcmurl; lcmurl << "file://" << in_log_fname << "?speed=" << processing_rate << "&start_timestamp=" << begin_timestamp; lcm_recv = new lcm::LCM(lcmurl.str()); if (!lcm_recv->good()) { fprintf(stderr, "Error couldn't load log file %s\n", lcmurl.str().c_str()); exit(1); } } else { lcm_recv = new lcm::LCM(bot_lcm_get_global(NULL)); } if (running_to_log) { printf("publishing into log file: %s\n", out_log_fname.c_str()); std::string lcmurl = "file://" + out_log_fname + "?mode=w"; lcm_pub = new lcm::LCM(lcmurl); if (!lcm_pub->good()) { fprintf(stderr, "Error couldn't open log file %s\n", lcmurl.c_str()); exit(1); } } else { lcm_pub = new lcm::LCM(); // mfallon publish back to lcm if run from log } if (param_fname.empty()) { param = bot_param_get_global(lcm_pub->getUnderlyingLCM(), 0); } else { param = bot_param_new_from_file(param_fname.c_str()); } if (param == NULL) { exit(1); } else if (!param_override_str.empty()) { int ret = bot_param_override_local_params(param, param_override_str.c_str()); if (ret <= 0) { fprintf(stderr, "Error overriding params with %s\n", param_override_str.c_str()); exit(1); } } char** active_sensor_names = bot_param_get_str_array_alloc(param, "state_estimator.active_sensors"); if (active_sensor_names == NULL) { fprintf(stderr, "Error: must specify active sensors using key state_estimator.active_sensors\n"); exit(1); } else { for (int i = 0; active_sensor_names[i]; i++) { active_sensors.insert(std::string(active_sensor_names[i])); } } bot_param_str_array_free(active_sensor_names); frames = bot_frames_get_global(lcm_recv->getUnderlyingLCM(), param); filter_state_channel = bot_param_get_str_or_fail(param, "state_estimator.filter_state_channel"); pose_channel = bot_param_get_str_or_fail(param, "state_estimator.pose_channel"); publish_filter_state = bot_param_get_boolean_or_fail(param, "state_estimator.publish_filter_state"); publish_pose = bot_param_get_boolean_or_fail(param, "state_estimator.publish_pose"); republish_sensors = bot_param_get_boolean_or_fail(param, "state_estimator.republish_sensors"); char *init_message_channel_char; char *init_complete_channel_char; if (bot_param_get_str(param, "state_estimator.init_message.channel", &init_message_channel_char) != 0) { // failed to get this key init_message_channel = ""; } else { init_message_channel = string(init_message_channel_char); free(init_message_channel_char); } if (bot_param_get_str(param, "state_estimator.init_message.init_complete_channel", &init_complete_channel_char) != 0) { // failed to get this key init_complete_channel = ""; } else { init_complete_channel = string(init_complete_channel_char); free(init_complete_channel_char); } exit_estimator = false; // when this is true, we exit the estimator handlers, mfallon }
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"); } }