KinectPointCloudPub::KinectPointCloudPub(int argc, char **argv) : m_threadExit(false)
{
// Declare the supported options.
po::options_description desc("Allowed options");
desc.add_options()
("help", "produce help message")
( "in",
po::value<std::string>(&m_kinectFrameName)->default_value(std::string("KINECT_FRAME")),
"input kinect LCM subscribe name")
( "out",
po::value<std::string>(&m_pointCloudName)->default_value(std::string("KINECT_POINTS")),
"output point cloud publication")
;
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm);
if (vm.count("help")) {
std::cout << desc << std::endl;
exit(1);
}
m_lcm = bot_lcm_get_global(NULL);
m_kinectSubscription = kinect_frame_msg_t_subscribe(m_lcm, m_kinectFrameName.c_str(), onKinectFrame, this);
m_calib = kinect_calib_new();
m_calib->width = 640;
m_calib->height = 480;
m_calib->intrinsics_depth.fx = 576.09757860;
m_calib->intrinsics_depth.cx = 321.06398107;
m_calib->intrinsics_depth.cy = 242.97676897;
m_calib->intrinsics_rgb.fx = 528.49404721;
m_calib->intrinsics_rgb.cx = 319.50000000;
m_calib->intrinsics_rgb.cy = 239.50000000;
m_calib->intrinsics_rgb.k1 = 0;
m_calib->intrinsics_rgb.k2 = 0;
m_calib->shift_offset = 1093.4753;
m_calib->projector_depth_baseline = 0.07214;;
double R[9] = { 0.999999, -0.000796, 0.001256, 0.000739, 0.998970, 0.045368, -0.001291, -0.045367, 0.998970 };
double T[3] = { -0.015756, -0.000923, 0.002316 };
memcpy(m_calib->depth_to_rgb_rot, R, 9*sizeof(double));
memcpy(m_calib->depth_to_rgb_translation, T, 3*sizeof(double));
m_pThread = new boost::thread(&KinectPointCloudPub::PubThread, this );
}
int main(int argc, char **argv)
{
setlinebuf(stdout);
g_thread_init(NULL);
app_t *app = (app_t*) calloc(1, sizeof(app_t));
app->lcm = bot_lcm_get_global(NULL);
app->ptr = new KinectPointCloudPub(argc, argv);
app_run(app);
app_destroy(app);
return 0;
}
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
}
/*
* setup_renderer:
* Generic renderer add function for use as a dynamically loaded plugin
*/
void add_renderer_to_plugin_viewer(BotViewer *viewer, int render_priority){
lcm_t * lcm = bot_lcm_get_global(NULL);
bot_lcmgl_add_renderer_to_viewer(viewer,lcm, render_priority);
}
