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