LaserGPFHandler::LaserGPFHandler(lcm_t * lcm, BotParam * param, BotFrames * frames)
{
lcm_pub = lcm;
gpf = new LaserGPF(lcm, param, frames);
char * pub_chan = bot_param_get_str_or_fail(param, "state_estimator.laser_gpf_out_of_process.channel");
pub_channel = pub_chan;
free(pub_chan);
char * laser_chan = bot_param_get_str_or_fail(param, "state_estimator.laser_gpf.channel");
laser_channel = laser_chan;
free(laser_chan);
}
LegOdoCommon::LegOdoCommon(lcm::LCM* lcm_recv, lcm::LCM* lcm_pub, BotParam * param):
lcm_recv(lcm_recv), lcm_pub(lcm_pub){
verbose = false;
char* mode_str = bot_param_get_str_or_fail(param, "state_estimator.legodo.mode");
if (strcmp(mode_str, "lin_rot_rate") == 0) {
mode_ = MODE_LIN_AND_ROT_RATE;
std::cout << "LegOdo will provide velocity and rotation rates." << std::endl;
}else if (strcmp(mode_str, "lin_rate") == 0){
mode_ = MODE_LIN_RATE;
std::cout << "LegOdo will provide linear velocity rates but no rotation rates." << std::endl;
}else if (strcmp(mode_str, "pos_and_lin_rate") == 0){
mode_ = MODE_POSITION_AND_LIN_RATE;
std::cout << "LegOdo will provide positions as well as linear velocity rates." << std::endl;
}else{
std::cout << "Legodo not understood! [LegOdoCommon]." << std::endl;
}
free(mode_str);
R_legodo_xyz_ = bot_param_get_double_or_fail(param, "state_estimator.legodo.r_xyz");
R_legodo_vxyz_ = bot_param_get_double_or_fail(param, "state_estimator.legodo.r_vxyz");
R_legodo_vang_ = bot_param_get_double_or_fail(param, "state_estimator.legodo.r_vang");
R_legodo_vxyz_uncertain_ = bot_param_get_double_or_fail(param, "state_estimator.legodo.r_vxyz_uncertain");
R_legodo_vang_uncertain_ = bot_param_get_double_or_fail(param, "state_estimator.legodo.r_vang_uncertain");
}
RgbdGPFHandler::RgbdGPFHandler(lcm_t * lcm, BotParam * param, BotFrames * frames)
{
lcm_pub = lcm;
gpf = new RgbdGPF(lcm, param, frames);
/// Insert out of process stuff here..
char * rgbd_chan = bot_param_get_str_or_fail(param, "state_estimator.rgbd_gpf.channel");
rgbd_channel = rgbd_chan;
free(rgbd_chan);
}
SensorInitHandler<lcmType, HandlerClass>::SensorInitHandler(
const std::string & _sensor_prefix, RBISInitializer * _initializer, HandlerFunction _handler_func,
HandlerClass * _handler)
{
handler_function = _handler_func;
handler = _handler;
initializer = _initializer;
sensor_prefix = _sensor_prefix;
char * chan = bot_param_get_str_or_fail(initializer->lcm_front->param,
("state_estimator." + sensor_prefix + ".channel").c_str());
channel = chan;
free(chan);
lcm_subscription = initializer->lcm_front->lcm_recv->subscribe(channel,
&SensorInitHandler<lcmType, HandlerClass>::message_handler, this);
fprintf(stderr, "subscribed to %s for sensor \"%s\" for initialization\n", channel.c_str(),
sensor_prefix.c_str());
}
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) {
bool ttl_one = false;
bool visualization = false;
bool traj_visualization = false;
std::string pose_channel = "STATE_ESTIMATOR_POSE";
std::string stereo_channel = "stereo";
std::string rc_trajectory_commands_channel = "rc-trajectory-commands";
std::string state_machine_go_autonomous_channel = "state-machine-go-autonomous";
std::string tvlqr_action_out_channel = "tvlqr-action";
std::string arm_for_takeoff_channel = "arm-for-takeoff";
std::string state_message_channel = "state-machine-state";
std::string altitude_reset_channel = "altitude-reset";
ConciseArgs parser(argc, argv);
parser.add(ttl_one, "t", "ttl-one", "Pass to set LCM TTL=1");
parser.add(pose_channel, "p", "pose-channel", "LCM channel to listen for pose messages on.");
parser.add(stereo_channel, "e", "stereo-channel", "LCM channel to listen to stereo messages on.");
parser.add(tvlqr_action_out_channel, "o", "tvlqr-out-channel", "LCM channel to publish which TVLQR trajectory is running on.");
parser.add(rc_trajectory_commands_channel, "r", "rc-trajectory-commands-channel", "LCM channel to listen for RC trajectory commands on.");
parser.add(state_machine_go_autonomous_channel, "a", "state-machine-go-autonomous-channel", "LCM channel to send go-autonmous messages on.");
parser.add(visualization, "v", "visualization", "Enables visualization of obstacles for HUD / LCMGL.");
parser.add(traj_visualization, "V", "traj-visualization", "Enables visualization of trajectories using LCMGL.");
parser.add(arm_for_takeoff_channel, "A", "arm-for-takeoff-channel", "LCM channel to receive arm for takeoff messages on.");
parser.add(state_message_channel, "s" "state-machine-state-channel", "LCM channel to send state machine state messages on.");
parser.parse();
std::string lcm_url;
// create an lcm instance
if (ttl_one) {
lcm_url = "udpm://239.255.76.67:7667?ttl=1";
} else {
lcm_url = "udpm://239.255.76.67:7667?ttl=0";
}
lcm::LCM lcm(lcm_url);
if (!lcm.good()) {
std::cerr << "LCM creation failed." << std::endl;
return 1;
}
BotParam *param = bot_param_new_from_server(lcm.getUnderlyingLCM(), 0);
std::string trajectory_dir = std::string(bot_param_get_str_or_fail(param, "tvlqr_controller.library_dir"));
trajectory_dir = ReplaceUserVarInPath(trajectory_dir);
StateMachineControl fsm_control(&lcm, trajectory_dir, tvlqr_action_out_channel, state_message_channel, altitude_reset_channel, visualization, traj_visualization);
//fsm_control.GetFsmContext()->setDebugFlag(true);
// subscribe to LCM channels
lcm.subscribe(pose_channel, &StateMachineControl::ProcessImuMsg, &fsm_control);
lcm.subscribe(stereo_channel, &StateMachineControl::ProcessStereoMsg, &fsm_control);
lcm.subscribe(rc_trajectory_commands_channel, &StateMachineControl::ProcessRcTrajectoryMsg, &fsm_control);
lcm.subscribe(state_machine_go_autonomous_channel, &StateMachineControl::ProcessGoAutonomousMsg, &fsm_control);
lcm.subscribe(arm_for_takeoff_channel, &StateMachineControl::ProcessArmForTakeoffMsg, &fsm_control);
omp_set_num_threads(3); // set the maximum number of threads
// to be 1 less than our number of
// cores so we never slow down
// the tvlqr process
printf("Receiving LCM:\n\tPose: %s\n\tStereo: %s\n\tRC Trajectories: %s\n\tGo Autonomous: %s\n\tArm for Takeoff: %s\n\nSending LCM:\n\tTVLQR Action: %s\n\tState Machine State: %s\n\tAltitude reset: %s\n", pose_channel.c_str(), stereo_channel.c_str(), rc_trajectory_commands_channel.c_str(), state_machine_go_autonomous_channel.c_str(), arm_for_takeoff_channel.c_str(), tvlqr_action_out_channel.c_str(), state_message_channel.c_str(), altitude_reset_channel.c_str());
while (true) {
while (NonBlockingLcm(lcm.getUnderlyingLCM())) {}
fsm_control.DoDelayedImuUpdate();
}
return 0;
}
//get lcm channel
char * bot_param_get_sensor_lcm_channel(BotParam *bot_param, const char * sensor_prefix, const char *sensor_name)
{
char key[1024];
snprintf(key, sizeof(key), "%s.%s.lcm_channel", sensor_prefix, sensor_name);
return bot_param_get_str_or_fail(bot_param, key);
}
//get coord frame name
char * bot_param_get_sensor_coord_frame(BotParam *bot_param, const char * sensor_prefix, const char *sensor_name)
{
char key[1024];
snprintf(key, sizeof(key), "%s.%s.coord_frame", sensor_prefix, sensor_name);
return bot_param_get_str_or_fail(bot_param, key);
}
