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
}
int main(int argc, char ** argv)
{
if (argc < 2) {
usage (argc, argv);
exit(1);
}
param_server_t * self = calloc(1, sizeof(param_server_t));
GMainLoop * mainloop = g_main_loop_new(NULL, FALSE);
if (!mainloop) {
fprintf (stderr, "Error getting the GLIB main loop\n");
exit(1);
}
char *optstring = "hs:l:m::";
struct option long_opts[] = {
{ "help", no_argument, NULL, 'h' },
{ "server-name", required_argument, NULL, 's' },
{ "lcm-url", required_argument, NULL, 'l' },
{ "message-init", optional_argument, NULL, 'm' },
{ 0, 0, 0, 0 }
};
int c=-1;
char *param_prefix = NULL;
char *lcm_url = NULL;
char *message_init_channel = NULL;
while ((c = getopt_long (argc, argv, optstring, long_opts, 0)) >= 0)
{
switch (c) {
case 's':
param_prefix = optarg;
break;
case 'l':
lcm_url = optarg;
break;
case 'm':
message_init_channel = (optarg==NULL) ? BOT_PARAM_UPDATE_CHANNEL : optarg;
break;
case 'h':
default:
usage (argc, argv);
return 1;
}
}
self->lcm = lcm_create(lcm_url);
lcmu_glib_mainloop_attach_lcm(self->lcm);
self->seqNo = 0;
self->id = _timestamp_now();
if (message_init_channel == NULL) {
self->params = bot_param_new_from_file(argv[optind]);
if (self->params==NULL){
fprintf(stderr, "Could not load params from %s\n", argv[optind]);
exit(1);
}
else{
fprintf(stderr, "Loaded params from %s\n", argv[optind]);
}
}
// set channels here
if (!param_prefix) param_prefix = getenv ("BOT_PARAM_SERVER_NAME");
self->update_channel = g_strconcat (param_prefix ? : "",
BOT_PARAM_UPDATE_CHANNEL, NULL);
self->request_channel = g_strconcat (param_prefix ? : "",
BOT_PARAM_REQUEST_CHANNEL, NULL);
self->set_channel = g_strconcat (param_prefix ? : "",
BOT_PARAM_SET_CHANNEL, NULL);
bot_param_update_t_subscribe(self->lcm, self->update_channel, on_param_update, (void *) self);
bot_param_request_t_subscribe(self->lcm, self->request_channel, on_param_request, (void *) self);
bot_param_set_t_subscribe(self->lcm, self->set_channel, on_param_set, (void *) self);
if (message_init_channel != NULL) {
fprintf(stderr, "Listening to %s for params\n", message_init_channel);
self->init_subscription = bot_param_update_t_subscribe(self->lcm, message_init_channel, on_param_init, (void *)self);
}
//timer to always publish params every 5sec
g_timeout_add_full(G_PRIORITY_HIGH, (guint) 5.0 * 1000, on_timer, (gpointer) self, NULL);
g_main_loop_run(mainloop);
return 0;
}
