/* ================ 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;
}
static void on_laser(const lcm_recv_buf_t *rbuf, const char *channel, const bot_core_planar_lidar_t *msg, void *user)
{
RendererLaser *self = (RendererLaser*) user;
g_assert(self);
/* get the laser channel object based on channel name */
laser_channel *lchan = g_hash_table_lookup(self->channels_hash, channel);
if (lchan == NULL) {
/* allocate and initialize laser channel structure */
lchan = (laser_channel*) calloc(1, sizeof(laser_channel));
g_assert(lchan);
lchan->enabled = 1;
lchan->name = bot_param_get_planar_lidar_name_from_lcm_channel(self->bot_param, channel);
lchan->channel = strdup(channel);
lchan->projector = laser_projector_new(self->bot_param, self->bot_frames, lchan->name, 1);
char param_prefix[1024];
bot_param_get_planar_lidar_prefix(NULL, lchan->name, param_prefix, sizeof(param_prefix));
char color_key[1024];
sprintf(color_key, "%s.viewer_color", param_prefix);
double color[3];
int color_size = bot_param_get_double_array(self->bot_param, color_key, color, 3);
if (color_size != 3) {
ERR("Error: Missing or funny color for planar LIDAR "
"configuration key: '%s'\n", color_key);
lchan->color[0] = 1;
lchan->color[1] = 1;
lchan->color[2] = 1;
}
else {
lchan->color[0] = color[0];
lchan->color[1] = color[1];
lchan->color[2] = color[2];
}
lchan->scans = bot_ptr_circular_new(self->param_max_buffer_size, laser_scan_destroy, NULL);
g_assert(lchan->scans);
/* add laser channel to hash table and array */
g_hash_table_insert(self->channels_hash, lchan->channel, lchan);
g_ptr_array_add(self->channels, lchan);
/* add check box */
if (self->viewer)
bot_gtk_param_widget_add_booleans(self->pw, 0, lchan->name, lchan->enabled, NULL);
}
/* TODO: Optimization - allocate space for local points from a
circular buffer instead of calling calloc for each scan */
laser_projected_scan *lscan = laser_create_projected_scan_from_planar_lidar(lchan->projector, msg,
bot_frames_get_root_name(self->bot_frames));
if (lscan == NULL)
return; //probably didn't have a pose message yet...
if(lchan->scans->capacity != self->param_max_buffer_size){
bot_ptr_circular_resize(lchan->scans, self->param_max_buffer_size);
}
if (bot_ptr_circular_size(lchan->scans) > 0) {
laser_projected_scan *last_scan = bot_ptr_circular_index(lchan->scans, 0);
/* check for a large time difference between scans (typical when
jumping around an LCM log file) */
gboolean time_jump = FALSE;
int64_t dt = msg->utime - last_scan->utime;
if (dt < -OLD_HISTORY_THRESHOLD || dt > OLD_HISTORY_THRESHOLD)
time_jump = TRUE;
/* spacial decimation */
gboolean stationary = FALSE;
BotTrans delta;
if (self->param_spacial_decimate && self->param_scan_memory > 10) {
bot_trans_invert_and_compose(&lscan->origin, &last_scan->origin, &delta);
double dist = bot_vector_magnitude_3d(delta.trans_vec);
double rot;
double axis[3];
bot_quat_to_angle_axis(delta.rot_quat, &rot, axis);
if (dist < SPACIAL_DECIMATION_LIMIT && rot < ANGULAR_DECIMATION_LIMIT) {
stationary = TRUE;
}
}
if (stationary) {
laser_scan_destroy(NULL, lscan);
return;
}
}
bot_ptr_circular_add(lchan->scans, lscan);
if (self->viewer)
bot_viewer_request_redraw(self->viewer);
return;
}
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;
}
