int main()
{
lcm_t * lcm = lcm_create(NULL);
BotParam * param = bot_param_new_from_server(lcm, 1);
if (param == NULL) {
fprintf(stderr, "could not get params!\n");
exit(1);
}
bot_param_add_update_subscriber(param,param_update_handler,lcm);
char * s;
int ret = bot_param_write_to_string(param, &s);
fprintf(stderr, "%s", s);
free(s);
// double foo = bot_param_get_double_or_fail(param, "foo");
// double bar = bot_param_get_double_or_fail(param, "bar");
// printf("foo=%f, bar = %f\n", foo, bar);
bot_param_write(param, stderr);
while (1) {
lcm_handle(lcm);
char * key = "coordinate_frames.body.history";
fprintf(stderr, "%s = %d\n", key, bot_param_get_int_or_fail(param, key));
}
return 0;
}
virtual void SetUp() {
// start up lcm
lcm_ = lcm_create ("udpm://239.255.76.67:7667?ttl=0");
param_ = bot_param_new_from_server(lcm_, 0);
converter_ = new ServoConverter(param_);
}
virtual void SetUp() {
lcm_ = new lcm::LCM("udpm://239.255.76.67:7667?ttl=0");
if (!lcm_->good()) {
std::cerr << "LCM creation failed." << std::endl;
exit(1);
}
// get parameter server
BotParam *param = bot_param_new_from_server(lcm_->getUnderlyingLCM(), 0);
bot_frames_ = bot_frames_new(lcm_->getUnderlyingLCM(), param);
}
virtual void SetUp() {
// start up lcm
lcm_ = lcm_create ("udpm://239.255.76.67:7667?ttl=0");
param_ = bot_param_new_from_server(lcm_, 0);
bot_frames_ = bot_frames_new(lcm_, param_);
bot_frames_get_trans(bot_frames_, "local", "opencvFrame", &global_to_camera_trans_);
bot_frames_get_trans(bot_frames_, "opencvFrame", "local", &camera_to_global_trans_);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
if (nrhs != 0) {
mexErrMsgIdAndTxt("drc:bot_param_get_server_id:arguments","Too many input arguments");
}
if (nlhs > 1) {
mexErrMsgIdAndTxt("drc:bot_param_get_server_id:arguments","Too many output arguments");
}
lcm::LCM lcm;
BotParam* botparam_;
botparam_ = bot_param_new_from_server(lcm.getUnderlyingLCM(), 0);
int64_t server_id;
if (botparam_==NULL) {
mexPrintf("bot-param server doesn't seem to be running... returning 0.");
server_id=0;
}
else {
server_id = bot_param_get_server_id(botparam_);
}
plhs[0] = mxCreateNumericMatrix(1,1,mxINT64_CLASS,mxREAL);
memcpy(mxGetData(plhs[0]),&server_id,sizeof(int));
return;
}
int main(int argc, char *argv[])
{
if (argc < 2) {
fprintf(stderr, "usage: %s <full vicon channel name> [frame name to update]\n", argv[0]);
exit(1);
}
app_t * app = (app_t *) calloc(1, sizeof(app_t));
app->vicon_channel = strdup(argv[1]);
app->body_frame_name = strdup("body");
if (argc > 2) {
app->body_frame_name = strdup(argv[2]);
}
app->lcm = lcm_create(NULL);
app->param = bot_param_new_from_server(app->lcm, 0);
app->frames = bot_frames_new(app->lcm, app->param);
vicon_body_t_subscribe(app->lcm, app->vicon_channel, vicon_handler, (void *) app);
while (true) {
int ret = lcm_handle(app->lcm);
}
}
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;
}
