Experiment.cpp
Go to the documentation of this file.
00001 /*
00002  * Experiment.cpp
00003  *
00004  *  Created on: Nov 15, 2011
00005  *      Author: mriedel
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 // Options
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                 // fail
00032                 ros::shutdown();
00033                 return;
00034         }
00035 
00036         //options.tUseMKInterface->setValue(true);
00037         if (options.tUseMKInterface->getValue()) {
00038                 ROS_INFO("Creating MKInterface!");
00039                 // use MKInterface
00040                 mkInterface = telekyb_interface::MKInterface::getMKInterface(options.robotID->getValue());
00041                 //mkInterface = telekyb_interface::MKInterface::getMKInterface(1); // BEWARE TEMPORARY!!!
00042                 if (!mkInterface) {
00043                         // fail
00044                         ros::shutdown();
00045                         return;
00046                 }
00047         }
00048 
00049         bController = core->getBehaviorController();
00050         oController = core->getOptionController();
00051 
00052         //activeBehavior = bController->getActiveBehaviorReference();
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         // load Behaviors
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         // sanity check
00078         if (ground.isNull() || hover.isNull() || normalBreak.isNull() || takeOff.isNull() || land.isNull()) {
00079                 ROS_FATAL("Unable to get SystemBehavior!!!");
00080                 //ROS_BREAK();
00081                 ros::shutdown();
00082         }
00083 
00084 
00085         // setup takeoff
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         // done
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 //      land.getOptionContainer().getOption("tLandDestinationHeight").set<double>(-0.25);
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                 //ROS_BREAK();
00106                 ros::shutdown();
00107         }
00108 
00109         // setup joystick
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                 //ROS_BREAK();
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         // lastly start Controller
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         // use button 2
00154         if (msg->buttons.size() < 9) {
00155                 ROS_ERROR("Jostick does not publish enough buttons.");
00156                 return;
00157         }
00158 
00159         // Emergency
00160         if (msg->buttons[6]) {
00161                 ROS_WARN("Emergency Button pressed!");
00162                 mkInterface->setEmergency();
00163         }
00164 
00165         // Button 1! toggle Motors for mkInterface Only in Ground
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                         // stop
00177                         motorState.value = MotorState::Off;
00178                         mkInterface->setMKValue(motorState);
00179                 } else if (motorState.value == MotorState::Off) {
00180                         // start
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                         // takeoff
00209                         bController->switchBehavior(takeOff);
00210                 } else {
00211                         // flying -> land
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         // what to do
00229 }
00230 
00231 void Experiment::activeBehaviorChanged(telekyb_interface::Behavior newActiveBehavior)
00232 {
00233 // Automatically turn off Motors
00234         if (mkInterface && newActiveBehavior == ground) {
00235                 MKSingleValuePacket motorState(MKDataDefines::MOTOR_STATE,0);
00236                 motorState.value = MotorState::Off;
00237                 mkInterface->setMKValue(motorState);
00238         }
00239 }
00240 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


tk_exp_joystick
Author(s): mriedel
autogenerated on Thu Apr 25 2013 11:16:01