int bot_param_get_planar_lidar_prefix(BotParam *param, const char *plidar_name, char *result, int result_size) { int n = snprintf(result, result_size, "%s.%s", PLANAR_LIDAR_PREFIX, plidar_name); if (n >= result_size) return -1; if (param) return bot_param_has_key(param, result) ? 0 : -1; else return 0; }
//get prefix for the sensors int bot_param_get_camera_prefix(BotParam *param, const char *cam_name, char *result, int result_size) { int n = snprintf(result, result_size, "%s.%s", CAMERA_PREFIX, cam_name); if (n >= result_size) return -1; if (param) return bot_param_has_key(param, result) ? 0 : -1; else return 0; }
/* ================ general ============== */ int bot_param_get_quat(BotParam *param, const char *name, double quat[4]) { char key[2048]; sprintf(key, "%s.quat", name); if (bot_param_has_key(param, key)) { int sz = bot_param_get_double_array(param, key, quat, 4); assert(sz == 4); return 0; } sprintf(key, "%s.rpy", name); if (bot_param_has_key(param, key)) { double rpy[3]; int sz = bot_param_get_double_array(param, key, rpy, 3); assert(sz == 3); for (int i = 0; i < 3; i++) rpy[i] = bot_to_radians (rpy[i]); bot_roll_pitch_yaw_to_quat(rpy, quat); return 0; } sprintf(key, "%s.rodrigues", name); if (bot_param_has_key(param, key)) { double rod[3]; int sz = bot_param_get_double_array(param, key, rod, 3); assert(sz == 3); bot_rodrigues_to_quat(rod, quat); return 0; } sprintf(key, "%s.angleaxis", name); if (bot_param_has_key(param, key)) { double aa[4]; int sz = bot_param_get_double_array(param, key, aa, 4); assert(sz == 4); bot_angle_axis_to_quat(aa[0], aa + 1, quat); return 0; } return -1; }
int bot_param_get_translation(BotParam *param, const char *name, double translation[3]) { char key[2048]; sprintf(key, "%s.translation", name); if (bot_param_has_key(param, key)) { int sz = bot_param_get_double_array(param, key, translation, 3); assert(sz == 3); return 0; } // not found return -1; }
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"); } }
BotCamTrans* bot_param_get_new_camtrans(BotParam *param, const char *cam_name) { char prefix[2048]; snprintf(prefix, sizeof(prefix), "%s.%s.intrinsic_cal", CAMERA_PREFIX, cam_name); if (!bot_param_has_key(param, prefix)) goto fail; char key[2048]; double width; sprintf(key, "%s.width", prefix); if (0 != bot_param_get_double(param, key, &width)) goto fail; double height; sprintf(key, "%s.height", prefix); if (0 != bot_param_get_double(param, key, &height)) goto fail; double pinhole_params[5]; snprintf(key, sizeof(key), "%s.pinhole", prefix); if (5 != bot_param_get_double_array(param, key, pinhole_params, 5)) goto fail; double fx = pinhole_params[0]; double fy = pinhole_params[1]; double cx = pinhole_params[3]; double cy = pinhole_params[4]; double skew = pinhole_params[2]; char * distortion_model; sprintf(key, "%s.distortion_model", prefix); if (0 != bot_param_get_str(param, key, &distortion_model)) goto fail; if (strcmp(distortion_model, "null") == 0) { BotDistortionObj* null_dist = bot_null_distortion_create(); BotCamTrans* null_camtrans = bot_camtrans_new(cam_name, width, height, fx, fy, cx, cy, skew, null_dist); return null_camtrans; } else if (strcmp(distortion_model, "spherical") == 0) { double distortion_param; sprintf(key, "%s.distortion_params", prefix); if (1 != bot_param_get_double_array(param, key, &distortion_param, 1)) goto fail; BotDistortionObj* sph_dist = bot_spherical_distortion_create(distortion_param); BotCamTrans* sph_camtrans = bot_camtrans_new(cam_name, width, height, fx, fy, cx, cy, skew, sph_dist); return sph_camtrans; } else if (strcmp(distortion_model, "plumb-bob") == 0) { double dist_k[3]; sprintf(key, "%s.distortion_k", prefix); if (3 != bot_param_get_double_array(param, key, dist_k, 3)) goto fail; double dist_p[2]; sprintf(key, "%s.distortion_p", prefix); if (2 != bot_param_get_double_array(param, key, dist_p, 2)) goto fail; BotDistortionObj* pb_dist = bot_plumb_bob_distortion_create(dist_k[0], dist_k[1], dist_k[2], dist_p[0], dist_p[1]); BotCamTrans* pb_camtrans = bot_camtrans_new(cam_name, width, height, fx, fy, cx, cy, skew, pb_dist); return pb_camtrans; } else if (strcmp(distortion_model, "angular-poly") == 0) { double coeffs[64]; sprintf(key, "%s.distortion_coeffs", prefix); int num_coeffs = bot_param_get_double_array(param, key, coeffs, -1); if (0 >= num_coeffs) goto fail; BotDistortionObj* ang_dist = bot_angular_poly_distortion_create(coeffs, num_coeffs); BotCamTrans* ang_camtrans = bot_camtrans_new(cam_name, width, height, fx, fy, cx, cy, skew, ang_dist); return ang_camtrans; } fail: return NULL; }