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); }