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