char * bot_param_get_sensor_name_from_lcm_channel(BotParam *param, const char * prefix, const char *channel)
{
char * sensor_name = NULL;
char **sensor_names = bot_param_get_subkeys(param, prefix);
for (int i = 0; sensor_names && sensor_names[i]; i++) {
char key[2048];
snprintf(key, sizeof(key), "%s.%s.lcm_channel", prefix, sensor_names[i]);
char *lcm_channel_str = NULL;
int key_status = bot_param_get_str(param, key, &lcm_channel_str);
if ((0 == key_status) && (0 == strcmp(channel, lcm_channel_str))) {
sensor_name = strdup(sensor_names[i]);
free(lcm_channel_str);
break;
}
else {
free(lcm_channel_str);
}
}
g_strfreev(sensor_names);
return sensor_name;
}
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
}
static BotRenderer* renderer_laser_new(BotViewer *viewer, lcm_t * lcm, BotParam * param, BotFrames * frames)
{
RendererLaser *self = (RendererLaser*) calloc(1, sizeof(RendererLaser));
g_assert(self);
self->viewer = viewer;
BotRenderer *renderer = &self->renderer;
renderer->draw = renderer_laser_draw;
renderer->destroy = renderer_laser_destroy;
renderer->user = self;
renderer->name = RENDERER_NAME;
renderer->enabled = 1;
self->lcm = lcm;
self->bot_param = param;
self->bot_frames = frames;
if (!self->lcm) {
ERR("Error: setup_renderer_laser() failed no LCM provided "
"object\n");
renderer_laser_destroy(renderer);
return NULL;
}
self->param_scan_memory = 50;
self->param_color_mode = COLOR_MODE_LASER;
self->param_max_buffer_size = MAX_SCAN_MEMORY;
self->param_z_buffer = TRUE;
self->param_big_points = FALSE;
self->param_spacial_decimate = FALSE;
self->param_color_mode_z_max_z = COLOR_MODE_Z_MAX_Z;
self->param_color_mode_z_min_z = COLOR_MODE_Z_MIN_Z;
self->param_max_draw_z = COLOR_MODE_Z_MAX_Z;
self->param_min_draw_z = COLOR_MODE_Z_MIN_Z;
self->param_max_draw_range = MAX_DRAW_RANGE;
self->param_alpha = 1;
self->param_max_buffer_size = MAX_SCAN_MEMORY;
if (viewer) {
/* setup parameter widget */
self->pw = BOT_GTK_PARAM_WIDGET(bot_gtk_param_widget_new());
renderer->widget = gtk_vbox_new(FALSE, 0);
gtk_box_pack_start(GTK_BOX(renderer->widget), GTK_WIDGET(self->pw), TRUE, TRUE, 0);
bot_gtk_param_widget_add_int(self->pw, PARAM_SCAN_MEMORY, BOT_GTK_PARAM_WIDGET_SPINBOX, 1, MAX_SCAN_MEMORY, 1,
self->param_scan_memory);
bot_gtk_param_widget_add_enum(self->pw, PARAM_COLOR_MODE, BOT_GTK_PARAM_WIDGET_MENU, self->param_color_mode,
"Laser", COLOR_MODE_LASER, "Drab", COLOR_MODE_DRAB, "Intensity", COLOR_MODE_INTENSITY, "Height", COLOR_MODE_Z, "Workspace", COLOR_MODE_WORKSPACE,
NULL);
bot_gtk_param_widget_add_booleans(self->pw, 0, PARAM_Z_BUFFER, self->param_z_buffer, NULL);
bot_gtk_param_widget_add_booleans(self->pw, 0, PARAM_BIG_POINTS, self->param_big_points, NULL);
bot_gtk_param_widget_add_booleans(self->pw, 0, PARAM_SPATIAL_DECIMATE, self->param_spacial_decimate, NULL);
bot_gtk_param_widget_add_booleans(self->pw, 0, PARAM_Z_RELATIVE, 0, NULL);
bot_gtk_param_widget_add_double(self->pw, PARAM_ALPHA, BOT_GTK_PARAM_WIDGET_SLIDER,
0, 1, .01, self->param_alpha);
bot_gtk_param_widget_add_double(self->pw, PARAM_COLOR_MODE_Z_MAX_Z, BOT_GTK_PARAM_WIDGET_SPINBOX,
COLOR_MODE_Z_MIN_Z, COLOR_MODE_Z_MAX_Z, COLOR_MODE_Z_DZ, self->param_color_mode_z_max_z);
bot_gtk_param_widget_add_double(self->pw, PARAM_COLOR_MODE_Z_MIN_Z, BOT_GTK_PARAM_WIDGET_SPINBOX,
COLOR_MODE_Z_MIN_Z, COLOR_MODE_Z_MAX_Z, COLOR_MODE_Z_DZ, self->param_color_mode_z_min_z);
bot_gtk_param_widget_add_double(self->pw, PARAM_MIN_DRAW_Z, BOT_GTK_PARAM_WIDGET_SPINBOX, COLOR_MODE_Z_MIN_Z,
COLOR_MODE_Z_MAX_Z, COLOR_MODE_Z_DZ, self->param_min_draw_z);
bot_gtk_param_widget_add_double(self->pw, PARAM_MAX_DRAW_Z, BOT_GTK_PARAM_WIDGET_SPINBOX, COLOR_MODE_Z_MIN_Z,
COLOR_MODE_Z_MAX_Z, COLOR_MODE_Z_DZ, self->param_max_draw_z);
bot_gtk_param_widget_add_double(self->pw, PARAM_MAX_DRAW_RANGE, BOT_GTK_PARAM_WIDGET_SPINBOX, 0,
MAX_DRAW_RANGE, 0.1, MAX_DRAW_RANGE); // from short to sick range
bot_gtk_param_widget_add_int(self->pw, PARAM_MAX_BUFFER_SIZE, BOT_GTK_PARAM_WIDGET_SPINBOX, 1, MAX_SCAN_MEMORY, 1,
self->param_max_buffer_size);
GtkWidget *clear_button = gtk_button_new_with_label("Clear memory");
gtk_box_pack_start(GTK_BOX(renderer->widget), clear_button, FALSE, FALSE, 0);
g_signal_connect(G_OBJECT(clear_button), "clicked", G_CALLBACK(on_clear_button), self);
GtkWidget *save_button = gtk_button_new_with_label("Save To Points File");
gtk_box_pack_start(GTK_BOX(renderer->widget), save_button, FALSE, FALSE, 0);
g_signal_connect(G_OBJECT(save_button), "clicked", G_CALLBACK(on_save_button), self);
gtk_widget_show_all(renderer->widget);
/* setup signal callbacks */
g_signal_connect(G_OBJECT(self->pw), "changed", G_CALLBACK(on_param_widget_changed), self);
g_signal_connect(G_OBJECT(viewer), "load-preferences", G_CALLBACK(on_load_preferences), self);
g_signal_connect(G_OBJECT(viewer), "save-preferences", G_CALLBACK(on_save_preferences), self);
}
// iterate through planar lidars, subscribing to thier LCM and
// initializing the channels' hash table and array
self->channels_hash = g_hash_table_new(g_str_hash, g_str_equal);
self->channels = g_ptr_array_new();
bot_core_planar_lidar_t_subscription_t *hid;
char **planar_lidar_names = bot_param_get_all_planar_lidar_names(self->bot_param);
if (planar_lidar_names) {
for (int pind = 0; planar_lidar_names[pind] != NULL; pind++) {
char conf_path[1024];
bot_param_get_planar_lidar_prefix(self->bot_param, planar_lidar_names[pind], conf_path, sizeof(conf_path));
char channel_path[1024];
sprintf(channel_path, "%s.lcm_channel", conf_path);
char * channel_name;
int ret = bot_param_get_str(self->bot_param, channel_path, &channel_name);
if (ret < 0) {
printf("No LCM Channel for lidar %s\n", planar_lidar_names[pind]);
continue;
}
printf("subscribing to channel %s for laser %s\n", channel_name, planar_lidar_names[pind]);
hid = bot_core_planar_lidar_t_subscribe(self->lcm, channel_name, on_laser, self);
self->lcm_hids = g_list_append(self->lcm_hids, hid);
free(channel_name);
}
g_strfreev(planar_lidar_names);
}
else {
fprintf(stderr, "["__FILE__":%d] Error: Could not"
" get lidar names.\n", __LINE__);
}
return &self->renderer;
}
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;
}
