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_);
lcmgl_ = bot_lcmgl_init(lcm_, "TrajectoryLibraryTest");
}
int main(int argc, char* argv[])
{
UNUSED(argc);
UNUSED(argv);
lcm_t * lcm;
lcm = lcm_create(NULL);
bot_lcmgl_t* lcmgl = bot_lcmgl_init(lcm, "lcmgl-example");
// bot_lcmgl_color3f(lcmgl, 0, 0, 1); // blue
double xyz[3] = {0.0, 0.0, 0.0};
bot_lcmgl_sphere(lcmgl, xyz, .1, 36, 36);
bot_lcmgl_switch_buffer(lcmgl);
bot_lcmgl_destroy(lcmgl);
return 0;
}
int main(int argc, char** argv)
{
lcm_t* lcm_ = lcm_create(NULL);
bot_lcmgl_t* lcmgl_ = bot_lcmgl_init(lcm_, "LCMGL_DEMO");
// lcmgl setup
bot_lcmgl_translated(lcmgl_, 0, 0, 0);
bot_lcmgl_line_width(lcmgl_, 2.0f);
bot_lcmgl_point_size(lcmgl_, 12.0f);
bot_lcmgl_begin(lcmgl_, GL_POINTS);
// setup color and draw a point
bot_lcmgl_color3f(lcmgl_, 1.0, 0, 0);
bot_lcmgl_vertex3f(lcmgl_, 0, 0, 1);
// end and refresh to viewer
bot_lcmgl_end(lcmgl_);
bot_lcmgl_switch_buffer(lcmgl_);
printf("Dosvedanya!\n");
return 0;
}
/**
* Tests an obstacle appearing during a trajectory execution
*/
TEST_F(StateMachineControlTest, TrajectoryInterrupt) {
StateMachineControl *fsm_control = new StateMachineControl(lcm_, "../TrajectoryLibrary/trajtest/full", "tvlqr-action-out", "state-machine-state", "altitude-reset", false);
//fsm_control->GetFsmContext()->setDebugFlag(true);
SubscribeLcmChannels(fsm_control);
ForceAutonomousMode();
float altitude = 100;
// send an obstacle to get it to transition to a new time
mav::pose_t msg = GetDefaultPoseMsg();
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
float point[3] = { 24, 0, 0+altitude };
SendStereoPointTriple(point);
ProcessAllLcmMessages(fsm_control);
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
// ensure that we have changed trajectories
EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 2); // from matlab
Trajectory running_traj = fsm_control->GetCurrentTrajectory();
// wait for that trajectory to time out
int64_t t_start = GetTimestampNow();
double t = 0;
while (t < 1.0) {
usleep(7142); // 1/140 of a second
msg.utime = GetTimestampNow();
t = (msg.utime - t_start) / 1000000.0;
Eigen::VectorXd state_t = running_traj.GetState(t);
msg.pos[0] = state_t(0);
msg.pos[1] = state_t(1);
msg.pos[2] = state_t(2) + altitude;
msg.vel[0] = state_t(6);
msg.vel[1] = state_t(7);
msg.vel[2] = state_t(8);
double rpy[3];
rpy[0] = state_t(3);
rpy[1] = state_t(4);
rpy[2] = state_t(5);
double quat[4];
bot_roll_pitch_yaw_to_quat(rpy, quat);
msg.orientation[0] = quat[0];
msg.orientation[1] = quat[1];
msg.orientation[2] = quat[2];
msg.orientation[3] = quat[3];
msg.rotation_rate[0] = state_t(9);
msg.rotation_rate[1] = state_t(10);
msg.rotation_rate[2] = state_t(11);
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
}
// now add a new obstacle right in front!
std::cout << "NEW POINT" << std::endl;
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
float point2[3] = { 18, 12, 0+altitude };
SendStereoPointTriple(point2);
ProcessAllLcmMessages(fsm_control);
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
fsm_control->GetOctomap()->Draw(lcm_->getUnderlyingLCM());
BotTrans body_to_local;
bot_frames_get_trans(bot_frames_, "body", "local", &body_to_local);
fsm_control->GetTrajectoryLibrary()->Draw(lcm_->getUnderlyingLCM(), &body_to_local);
fsm_control->GetTrajectoryLibrary()->Draw(lcm_->getUnderlyingLCM(), &body_to_local);
bot_lcmgl_t *lcmgl = bot_lcmgl_init(lcm_->getUnderlyingLCM(), "Trjaectory");
bot_lcmgl_color3f(lcmgl, 0, 1, 0);
fsm_control->GetCurrentTrajectory().Draw(lcmgl ,&body_to_local);
bot_lcmgl_switch_buffer(lcmgl);
bot_lcmgl_destroy(lcmgl);
EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 3); // from matlab
delete fsm_control;
UnsubscribeLcmChannels();
}
