Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #include "Experiment.hpp"
00009
00010 #include <telekyb_base/ROS.hpp>
00011 #include <telekyb_base/Time.hpp>
00012
00013 #include <telekyb_base/Spaces.hpp>
00014
00015
00016 ExperimentOptions::ExperimentOptions()
00017 : OptionContainer("ExperimentOptions")
00018 {
00019 robotID = addOption<int>("robotID", "Specify the robotID of the TeleKybCore to connect to.", 0, false, true);
00020 tJoystickTopic = addOption<std::string>("tJoystickTopic",
00021 "Joysticktopic to use (sensor_msgs::Joy)", "/TeleKyb/tJoy/joy", false, true);
00022 tUseMKInterface = addOption<bool>("tUseMKInterface", "Set to true with MKInterface!", false, false, true);
00023 trajectoryFile = addOption<std::string>("trajectoryFile", "Trajectory file", "", false, true);
00024 }
00025
00026 Experiment::Experiment()
00027 : mainNodeHandle( ROSModule::Instance().getMainNodeHandle() ), core(NULL), mkInterface(NULL)
00028 {
00029 core = telekyb_interface::TeleKybCore::getTeleKybCore(options.robotID->getValue());
00030 if (!core) {
00031
00032 ros::shutdown();
00033 return;
00034 }
00035
00036
00037 if (options.tUseMKInterface->getValue()) {
00038 ROS_INFO("Creating MKInterface!");
00039
00040 mkInterface = telekyb_interface::MKInterface::getMKInterface(options.robotID->getValue());
00041
00042 if (!mkInterface) {
00043
00044 ros::shutdown();
00045 return;
00046 }
00047 }
00048
00049 bController = core->getBehaviorController();
00050 oController = core->getOptionController();
00051
00052
00053 bController->setActiveBehaviorListener(this);
00054
00055 activeBehaviorPtr = bController->getActiveBehaviorPointer();
00056
00057 setupExperiment();
00058 }
00059
00060 Experiment::~Experiment()
00061 {
00062 if (mkInterface) { delete mkInterface; }
00063
00064 delete core;
00065 }
00066
00067 void Experiment::setupExperiment()
00068 {
00069
00070 ground = bController->getSystemBehavior("tk_behavior/Ground");
00071 hover = bController->getSystemBehavior("tk_behavior/Hover");
00072 normalBreak = bController->getSystemBehavior("tk_behavior/NormalBrake");
00073 takeOff = bController->getSystemBehavior("tk_behavior/TakeOff");
00074 land = bController->getSystemBehavior("tk_behavior/Land");
00075
00076
00077
00078 if (ground.isNull() || hover.isNull() || normalBreak.isNull() || takeOff.isNull() || land.isNull()) {
00079 ROS_FATAL("Unable to get SystemBehavior!!!");
00080
00081 ros::shutdown();
00082 }
00083
00084
00085
00086 takeOff.getOptionContainer().getOption("tTakeOffVelocity").set<double>(0.3);
00087 takeOff.getOptionContainer().getOption("tTakeOffDestination").set(Position3D(0.0,0.0,-1.0));
00088 takeOff.getOptionContainer().getOption("tTakeOffVertically").set(true);
00089
00090
00091 takeOff.setParameterInitialized(true);
00092
00093
00094 land.getOptionContainer().getOption("tLandVelocity").set<double>(0.4);
00095 land.getOptionContainer().getOption("tLandDestination").set(Position3D(0.0,0.0,0.0));
00096
00097 land.setParameterInitialized(true);
00098
00099
00100 joystick = bController->loadBehavior("tk_be_common/Joystick");
00101 trajPlayback = bController->loadBehavior("tk_be_common/TrajPlayback");
00102
00103 if ( joystick.isNull() ) {
00104 ROS_FATAL("Unable to load Joystick!!!");
00105
00106 ros::shutdown();
00107 }
00108
00109
00110 joystick.getOptionContainer().getOption("tJoystickTopic").set(options.tJoystickTopic->getValue());
00111 joystick.setParameterInitialized(true);
00112
00113 if ( trajPlayback.isNull() ) {
00114 ROS_FATAL("Unable to load Trajectory Playback!!!");
00115
00116 ros::shutdown();
00117 }
00118
00119 trajPlayback.getOptionContainer().getOption("tTrajectoryFilename").set(options.trajectoryFile->getValue());
00120 trajPlayback.setParameterInitialized(true);
00121
00122 flyto1 = bController->loadBehavior("tk_be_common/LinearFlyTo");
00123 flyto2 = bController->loadBehavior("tk_be_common/LinearFlyTo");
00124 flyto3 = bController->loadBehavior("tk_be_common/LinearFlyTo");
00125
00126 flyto1.getOptionContainer().getOption("tFlyToDestination").set(Position3D(0.5,0.5,-1.5));
00127 flyto2.getOptionContainer().getOption("tFlyToDestination").set(Position3D(0.5,-0.5,-0.5));
00128 flyto3.getOptionContainer().getOption("tFlyToDestination").set(Position3D(-0.5,0.0,-1.0));
00129
00130 flyto1.setParameterInitialized(true);
00131 flyto2.setParameterInitialized(true);
00132 flyto3.setParameterInitialized(true);
00133
00134 flyto1.setNextBehavior(flyto2);
00135 flyto2.setNextBehavior(flyto3);
00136 flyto3.setNextBehavior(flyto1);
00137
00138 if (*activeBehaviorPtr != ground) {
00139 ROS_ERROR("UAV not in Ground Behavior during Startup");
00140 ros::shutdown();
00141 }
00142
00143
00144 joySub = mainNodeHandle.subscribe(options.tJoystickTopic->getValue()
00145 , 10, &Experiment::joystickCB, this);
00146 }
00147
00148
00149
00150
00151 void Experiment::joystickCB(const sensor_msgs::Joy::ConstPtr& msg)
00152 {
00153
00154 if (msg->buttons.size() < 9) {
00155 ROS_ERROR("Jostick does not publish enough buttons.");
00156 return;
00157 }
00158
00159
00160 if (msg->buttons[6]) {
00161 ROS_WARN("Emergency Button pressed!");
00162 mkInterface->setEmergency();
00163 }
00164
00165
00166 if (mkInterface && *activeBehaviorPtr == ground && msg->buttons[0]) {
00167 ROS_INFO("Toggle Motorstate!");
00168
00169 MKSingleValuePacket motorState(MKDataDefines::MOTOR_STATE,0);
00170 if (!mkInterface->updateMKValue(motorState)) {
00171 ROS_ERROR("Could not get Motorstate!");
00172 return;
00173 }
00174
00175 if (motorState.value == MotorState::On) {
00176
00177 motorState.value = MotorState::Off;
00178 mkInterface->setMKValue(motorState);
00179 } else if (motorState.value == MotorState::Off) {
00180
00181 motorState.value = MotorState::Init;
00182 mkInterface->setMKValue(motorState);
00183 } else if (motorState.value == MotorState::Init) {
00184 motorState.value = MotorState::On;
00185 mkInterface->setMKValue(motorState);
00186 }
00187
00188
00189 }
00190
00191 if (msg->buttons[2]) {
00192 if (*activeBehaviorPtr == ground) {
00193
00194 if (mkInterface) {
00195 MKSingleValuePacket motorState(MKDataDefines::MOTOR_STATE,0);
00196 if (!mkInterface->updateMKValue(motorState)) {
00197 ROS_ERROR("Could not get Motorstate for liftoff!");
00198 return;
00199 }
00200 if (motorState.value == MotorState::On) {
00201 bController->switchBehavior(takeOff);
00202 } else {
00203 ROS_ERROR("Motors have to be on for liftOff!");
00204 return;
00205 }
00206 }
00207
00208
00209 bController->switchBehavior(takeOff);
00210 } else {
00211
00212 bController->switchBehavior(land);
00213 }
00214 }
00215
00216 if (msg->buttons[3]) {
00217 bController->switchBehavior(joystick);
00218 }
00219
00220 if (msg->buttons[4]) {
00221 bController->switchBehavior(trajPlayback);
00222 }
00223
00224 if (msg->buttons[1]) {
00225 bController->switchBehavior(flyto1);
00226 }
00227
00228
00229 }
00230
00231 void Experiment::activeBehaviorChanged(telekyb_interface::Behavior newActiveBehavior)
00232 {
00233
00234 if (mkInterface && newActiveBehavior == ground) {
00235 MKSingleValuePacket motorState(MKDataDefines::MOTOR_STATE,0);
00236 motorState.value = MotorState::Off;
00237 mkInterface->setMKValue(motorState);
00238 }
00239 }
00240