void
Visualization::draw(const VisualOdometry* odom)
{
const OdometryFrame* target_frame = odom->getTargetFrame();
int num_levels = target_frame->getNumLevels();
int base_width = target_frame->getLevel(0)->getWidth();
int base_height = target_frame->getLevel(0)->getHeight();
bot_lcmgl_push_matrix(_lcmgl);
bot_lcmgl_rotated(_lcmgl, -90, 0, 0, 1);
bot_lcmgl_scalef(_lcmgl, 10.0 / base_width, -10.0 / base_width, 1);
int x_offset = 0;
for (int i=0; i<num_levels; i++) {
x_offset += (2*base_width >> i) + 10;
bot_lcmgl_push_matrix(_lcmgl);
bot_lcmgl_translated(_lcmgl, x_offset, 0, 0);
draw_pyramid_level_matches(odom, i);
bot_lcmgl_pop_matrix(_lcmgl);
}
x_offset = 0;
for (int i=0; i<num_levels; i++) {
x_offset += (2*base_width >> i) + 10;
bot_lcmgl_push_matrix(_lcmgl);
bot_lcmgl_translated(_lcmgl, x_offset, 2*base_height+10, 0);
draw_pyramid_level_flow(odom, i);
bot_lcmgl_pop_matrix(_lcmgl);
}
bot_lcmgl_pop_matrix(_lcmgl);
bot_lcmgl_switch_buffer(_lcmgl);
}
TEST_F(TrajectoryLibraryTest, ManyTrajectoriesWithTransform) {
StereoOctomap octomap(bot_frames_);
TrajectoryLibrary lib(0);
lib.LoadLibrary("trajtest/full", true);
double altitude = 30;
double point[3] = {18, 12, 0};
AddPointToOctree(&octomap, point, altitude);
BotTrans trans;
bot_trans_set_identity(&trans);
trans.trans_vec[0] = 17;
trans.trans_vec[1] = 11;
trans.trans_vec[2] = altitude;
// send th
double dist;
const Trajectory *best_traj;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0);
DrawOriginLcmGl(lcm_);
bot_lcmgl_color3f(lcmgl_, 0, 0, 1);
best_traj->Draw(lcmgl_, &trans);
EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 3);
EXPECT_NEAR(dist, 1.025243, TOLERANCE);
// now add a yaw
trans.rot_quat[0] = 0.642787609686539;
trans.rot_quat[1] = 0;
trans.rot_quat[2] = 0;
trans.rot_quat[3] = 0.766044443118978;
bot_lcmgl_color3f(lcmgl_, 1, 0, 0);
best_traj->Draw(lcmgl_, &trans);
bot_lcmgl_switch_buffer(lcmgl_);
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0);
//lib.Draw(lcm_, &trans);
EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 2);
EXPECT_NEAR(dist, 1.174604, TOLERANCE);
// now have a transform with roll, pitch, and yaw
trans.rot_quat[0] = 0.863589399067779;
trans.rot_quat[1] = -0.004581450790098;
trans.rot_quat[2] = 0.298930259006064;
trans.rot_quat[3] = 0.405996379758463;
std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0);
EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 4);
EXPECT_NEAR(dist, 0.327772, TOLERANCE);
}
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();
}
