void
bot_roll_pitch_yaw_to_angle_axis (const double rpy[3], double *theta,
double axis[3])
{
double q[4];
bot_roll_pitch_yaw_to_quat(rpy, q);
bot_quat_to_angle_axis (q, theta, axis);
}
/* ================ general ============== */
int bot_param_get_quat(BotParam *param, const char *name, double quat[4])
{
char key[2048];
sprintf(key, "%s.quat", name);
if (bot_param_has_key(param, key)) {
int sz = bot_param_get_double_array(param, key, quat, 4);
assert(sz == 4);
return 0;
}
sprintf(key, "%s.rpy", name);
if (bot_param_has_key(param, key)) {
double rpy[3];
int sz = bot_param_get_double_array(param, key, rpy, 3);
assert(sz == 3);
for (int i = 0; i < 3; i++)
rpy[i] = bot_to_radians (rpy[i]);
bot_roll_pitch_yaw_to_quat(rpy, quat);
return 0;
}
sprintf(key, "%s.rodrigues", name);
if (bot_param_has_key(param, key)) {
double rod[3];
int sz = bot_param_get_double_array(param, key, rod, 3);
assert(sz == 3);
bot_rodrigues_to_quat(rod, quat);
return 0;
}
sprintf(key, "%s.angleaxis", name);
if (bot_param_has_key(param, key)) {
double aa[4];
int sz = bot_param_get_double_array(param, key, aa, 4);
assert(sz == 4);
bot_angle_axis_to_quat(aa[0], aa + 1, quat);
return 0;
}
return -1;
}
TEST_F(StateMachineControlTest, BearingController) {
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);
// ensure that we are in the start state
EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::WaitForTakeoff") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName();
// send some pose messages
mav::pose_t msg = GetDefaultPoseMsg();
msg.pos[2] = 0;
// check for unexpected transitions out of wait state
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::WaitForTakeoff") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName();
lcm_->publish(pose_channel_, &msg);
lcm_->publish(pose_channel_, &msg);
lcm_->publish(pose_channel_, &msg);
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
// cause a takeoff transistion
msg.accel[0] = 100;
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::TakeoffNoThrottle") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName();
// wait for takeoff to finish
for (int i = 0; i < 12; i++) {
msg = GetDefaultPoseMsg();
msg.pos[2] = 0;
msg.vel[0] = 20;
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
usleep(100000);
}
EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::Climb") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName();
// reach altitude
msg = GetDefaultPoseMsg();
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
EXPECT_TRUE(fsm_control->GetCurrentStateName().compare("AirplaneFsm::ExecuteTrajectory") == 0) << "In wrong state: " << fsm_control->GetCurrentStateName();
EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 0);
// now add an obstacle in front of it and make sure it transitions!
ProcessAllLcmMessages(fsm_control);
ProcessAllLcmMessages(fsm_control);
// send message indicating that we are turned to the right
msg = GetDefaultPoseMsg();
double rpy[3] = { 0, 0, -0.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];
lcm_->publish(pose_channel_, &msg);
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 3); // 3 = left turn controller
// check for stop
msg = GetDefaultPoseMsg();
lcm_->publish(pose_channel_, &msg);
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 0);
// check for right turn
msg = GetDefaultPoseMsg();
double rpy2[3] = { 0, 0, 0.5 };
double quat2[4];
bot_roll_pitch_yaw_to_quat(rpy2, quat2);
msg.orientation[0] = quat2[0];
msg.orientation[1] = quat2[1];
msg.orientation[2] = quat2[2];
msg.orientation[3] = quat2[3];
lcm_->publish(pose_channel_, &msg);
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 4); // 4 = right turn controller
// check for stop
msg = GetDefaultPoseMsg();
lcm_->publish(pose_channel_, &msg);
lcm_->publish(pose_channel_, &msg);
ProcessAllLcmMessages(fsm_control);
EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 0);
delete fsm_control;
UnsubscribeLcmChannels();
}
/**
* 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();
}
static void on_param_widget_changed(BotGtkParamWidget *pw, const char *name, void *user)
{
RendererFrames *self = (RendererFrames*) user;
if (self->updating) {
return;
}
BotViewer *viewer = self->viewer;
int activeSensorNum = bot_gtk_param_widget_get_enum(pw, PARAM_FRAME_SELECT);
if (!strcmp(name, PARAM_FRAME_SELECT)) {
if (activeSensorNum > 0) {
self->updating = 1;
bot_viewer_set_status_bar_message(self->viewer, "Modify Calibration relative to %s", bot_frames_get_relative_to(
self->frames, self->frameNames[activeSensorNum]));
bot_gtk_param_widget_set_enabled(self->pw, PARAM_X, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Y, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Z, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_ROLL, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_PITCH, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_YAW, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_SAVE, 1);
frames_update_handler(self->frames, self->frameNames[activeSensorNum], bot_frames_get_relative_to(self->frames,
self->frameNames[activeSensorNum]), bot_timestamp_now(), self);
}
else {
bot_viewer_set_status_bar_message(self->viewer, "");
bot_gtk_param_widget_set_enabled(self->pw, PARAM_X, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Y, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Z, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_ROLL, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_PITCH, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_YAW, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_SAVE, 0);
}
}
else if (!strcmp(name, PARAM_SAVE) && activeSensorNum > 0) {
char save_fname[1024];
sprintf(save_fname, "manual_calib_%s.cfg", self->frameNames[activeSensorNum]);
fprintf(stderr, "saving params to: %s\n", save_fname);
FILE * f = fopen(save_fname, "w");
double pos[3];
pos[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_X);
pos[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_Y);
pos[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_Z);
double rpy[3];
rpy[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_ROLL);
rpy[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_PITCH);
rpy[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_YAW);
double rpy_rad[3];
for (int i = 0; i < 3; i++)
rpy_rad[i] = bot_to_radians(rpy[i]);
double quat[4];
bot_roll_pitch_yaw_to_quat(rpy_rad, quat);
double rod[3];
bot_quat_to_rodrigues(quat, rod);
fprintf(f, ""
"%s {\n"
"position = [%f, %f, %f];\n"
"rpy = [%f, %f, %f];\n"
"relative_to = \"%s\";\n"
"}", self->frameNames[activeSensorNum], pos[0], pos[1], pos[2], rpy[0], rpy[1], rpy[2],
bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum]));
fprintf(f, ""
"\n"
"%s {\n"
"position = [%f, %f, %f];\n"
"rodrigues = [%f, %f, %f];\n"
"relative_to = \"%s\";\n"
"}", self->frameNames[activeSensorNum], pos[0], pos[1], pos[2], rod[0], rod[1], rod[2],
bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum]));
fclose(f);
bot_viewer_set_status_bar_message(self->viewer, "Calibration saved to %s", save_fname);
}
else if (activeSensorNum > 0) {
self->updating = 1;
BotTrans curr;
curr.trans_vec[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_X);
curr.trans_vec[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_Y);
curr.trans_vec[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_Z);
double rpy[3];
rpy[0] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_ROLL));
rpy[1] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_PITCH));
rpy[2] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_YAW));
bot_roll_pitch_yaw_to_quat(rpy, curr.rot_quat);
if (fabs(rpy[0]) > M_PI || fabs(rpy[1]) > M_PI || fabs(rpy[2]) > M_PI) {
bot_gtk_param_widget_set_double(self->pw, PARAM_ROLL, bot_to_degrees(bot_mod2pi(rpy[0])));
bot_gtk_param_widget_set_double(self->pw, PARAM_PITCH, bot_to_degrees(bot_mod2pi(rpy[1])));
bot_gtk_param_widget_set_double(self->pw, PARAM_YAW, bot_to_degrees(bot_mod2pi(rpy[2])));
}
//and update the link
const char * frame_name = self->frameNames[activeSensorNum];
const char * relative_to = bot_frames_get_relative_to(self->frames, frame_name);
bot_frames_update_frame(self->frames, frame_name, relative_to, &curr, bot_timestamp_now());
}
bot_viewer_request_redraw(self->viewer);
}
