Experiment.cpp

Go to the documentation of this file.

/*
 * Experiment.cpp
 *
 *  Created on: Nov 15, 2011
 *      Author: mriedel
 */

#include "Experiment.hpp"

#include <telekyb_base/ROS.hpp>
#include <telekyb_base/Time.hpp>

#include <telekyb_base/Spaces.hpp>

#include <boost/algorithm/string.hpp>

#define FLYTO_Z -0.75

namespace telekyb {

Experiment::Experiment()
        : currState(State::Init),
          mainNodeHandle( ROSModule::Instance().getMainNodeHandle() ),
          system(NULL),
          mkInterface(NULL),
          neutralPosSet(false)
{
        system = telekyb_interface::TeleKybSystem::getTeleKybSystem(options.robotID->getValue());
        if (!system) {
                // fail
                ros::shutdown();
                return;
        }

        //options.tUseMKInterface->setValue(true);
        if (options.tUseMKInterface->getValue()) {
                ROS_INFO("Creating MKInterface!");
                // use MKInterface
                mkInterface = telekyb_interface::MKInterface::getMKInterface(options.robotID->getValue());
                //mkInterface = telekyb_interface::MKInterface::getMKInterface(1); // BEWARE TEMPORARY!!!
                if (!mkInterface) {
                        // fail
                        ros::shutdown();
                        return;
                }
        }

        bController = system->getBehaviorController();
        oController = system->getOptionController();

        //activeBehavior = bController->getActiveBehaviorReference();

        bController->setActiveBehaviorListener(this);
        activeBehaviorPtr = bController->getActiveBehaviorPointer();

        // UDP to Virtools
        udp.initSender(options.tClientHostname->getValue().c_str(), options.tClientPort->getValue());
        udp.initReceiver(options.tClientPort->getValue());

        setupExperiment();
}

Experiment::~Experiment()
{
        if (mkInterface) { delete mkInterface; }

        delete system;
}

void Experiment::setupExperiment()
{
        // load Behaviors
        ground = bController->getSystemBehavior("tk_behavior/Ground");
        hover = bController->getSystemBehavior("tk_behavior/Hover");
        normalBreak = bController->getSystemBehavior("tk_behavior/NormalBrake");
        takeOff = bController->getSystemBehavior("tk_behavior/TakeOff");
        land = bController->getSystemBehavior("tk_behavior/Land");


        // sanity check
        if (ground.isNull() || hover.isNull() || normalBreak.isNull() || takeOff.isNull() || land.isNull()) {
                ROS_FATAL("Unable to get SystemBehavior!!!");
                //ROS_BREAK();
                ros::shutdown();
        }

        // setup takeoff
        takeOff.getOptionContainer().getOption("tTakeOffDestination").set(Position3D(0.0,0.0,-1.0));
        takeOff.getOptionContainer().getOption("tTakeOffVertically").set(true);
        // done
        takeOff.setParameterInitialized(true);

        land.getOptionContainer().getOption("tLandDestination").set(Position3D(0.0,0.0,0.0));
        land.getOptionContainer().getOption("tLandDestinationHeight").set(-0.15);
        land.getOptionContainer().getOption("tLandVertically").set(true); // actually is default!
        // done
        land.setParameterInitialized(true);

        handJoystick = bController->loadBehavior("tk_vrqc2011/HandJoystick");

        if ( handJoystick.isNull() ) {
                ROS_FATAL("Unable to load HandJoystick!!!");
                //ROS_BREAK();
                ros::shutdown();
        }

        // setup joystick
        handJoystick.getOptionContainer().getOption("tHandJoystickTopic").set(options.tHandJoystickTopic->getValue());
        handJoystick.getOptionContainer().getOption("tHandJoystickScale").set(1.0); // 1m/s
        handJoystick.setNextBehavior(land);
        handJoystick.setParameterInitialized(true);

        // takeoff next Behavior
        takeOff.setNextBehavior(handJoystick);

        flyToTakeOff = bController->loadBehavior("tk_behavior/TakeOff");
        flyTo = bController->loadBehavior("tk_be_common/DynamicFlyTo");
        flyToLand = bController->loadBehavior("tk_behavior/Land");
        if (flyToTakeOff.isNull() || flyTo.isNull() || flyToLand.isNull()) {
                ROS_FATAL("Unable to get FlyTo Behaviors!!!");
                //ROS_BREAK();
                ros::shutdown();
        }

        // Configure Flyto Behaviors
        flyToTakeOff.getOptionContainer().getOption("tTakeOffDestination").set(Position3D(0.0,0.0,FLYTO_Z));
        flyToTakeOff.getOptionContainer().getOption("tTakeOffVertically").set(true);
        flyToTakeOff.getOptionContainer().getOption("tTakeOffVelocity").set(0.4);
        flyToTakeOff.getOptionContainer().getOption("tTakeOffDestinationRadius").set(0.3);

        flyToTakeOff.setNextBehavior(flyTo);

        // done
        flyToTakeOff.setParameterInitialized(true);

        // This get's set dynamically
        flyTo.getOptionContainer().getOption("tFlyToDestination").set(Position3D(0.0,0.0,FLYTO_Z));
        flyTo.getOptionContainer().getOption("tFlyToVelocity").set(1.2);
        flyTo.getOptionContainer().getOption("tFlyToOuterDestinationRadius").set(0.40); // 40cm
        flyTo.getOptionContainer().getOption("tFlyToInnerDestinationRadius").set(0.03); // 5cm

        flyTo.setNextBehavior(flyToLand);
        flyTo.setParameterInitialized(true);

        flyToLand.getOptionContainer().getOption("tLandDestination").set(Position3D(0.0,0.0,0.0));
        flyToLand.getOptionContainer().getOption("tLandDestinationHeight").set(-0.15);
        flyToLand.getOptionContainer().getOption("tLandVelocity").set(0.4);
        flyToLand.getOptionContainer().getOption("tLandVertically").set(false); // actually is default!

        // done
        flyToLand.setNextBehavior(ground);
        flyToLand.setParameterInitialized(true);


        if (*activeBehaviorPtr != ground) {
                ROS_ERROR("UAV not in Ground Behavior during Startup");
                ros::shutdown();
        }

        // lastly start Controller
        joySub = mainNodeHandle.subscribe(options.tJoystickTopic->getValue()
                        , 10, &Experiment::joystickCB, this);

        // HandJoy
        handJoySub = mainNodeHandle.subscribe(options.tHandJoystickTopic->getValue()
                        , 10, &Experiment::handJoystickCB, this);

        // For Testing
        srand((unsigned)time(NULL));
}




void Experiment::joystickCB(const sensor_msgs::Joy::ConstPtr& msg)
{
        // use button 2
        if (msg->buttons.size() < 10) {
                ROS_ERROR("Jostick does not publish enough buttons.");
                return;
        }

        // Emergency
        if (msg->buttons[6]) {
                ROS_WARN("Emergency Button pressed!");
                mkInterface->setEmergency();
        }

        // Button 1! toggle Motors for mkInterface Only in Ground
        if (mkInterface && *activeBehaviorPtr == ground && msg->buttons[0]) {

                MKSingleValuePacket motorState(MKDataDefines::MOTOR_STATE,0);
                if (!mkInterface->updateMKValue(motorState)) {
                        ROS_ERROR("Could not get Motorstate!");
                        return;
                }

                if (motorState.value == MotorState::On) {
                        // stop
                        motorState.value = MotorState::Off;
                        mkInterface->setMKValue(motorState);
                } else if (motorState.value == MotorState::Off) {
                        // start
                        motorState.value = MotorState::Init;
                        mkInterface->setMKValue(motorState);
                } else if (motorState.value == MotorState::Init) {
                        motorState.value = MotorState::On;
                        mkInterface->setMKValue(motorState);
                }


        }

        if (msg->buttons[2]) {
                ROS_ERROR("LiftOff/Land Button.");
                if (*activeBehaviorPtr == ground) {
                        // takeoff
                        bController->switchBehavior(takeOff);
                } else {
                        // flying -> land
                        bController->switchBehavior(land);
                }
        }

        // Test Error
        if (msg->buttons[3]) {
                // ROS_INFO("Received Joystick CB");
                //bController->switchBehavior(handJoystick);
                changeState("#error#There was an error.#", State::Error);
        }

        if (msg->buttons[4]) {
                ROS_INFO("Received Joystick CB");
                //ROS_INFO("Size: %d, Value: %d", msg->buttons.size(), msg->buttons.at(0));
                ROS_INFO("CurrentState: %s", currState.str());
                std::string command;
                // Start Experiment
                if (currState == State::Init) {
                        changeState("#startExp#", State::Init);
                }
        }

        // right back. Testing FlyTo
        if (msg->buttons[5]) {
                double flyToX = (((double)rand() / RAND_MAX) - 0.5) * 4.0;
                double flyToY = (((double)rand() / RAND_MAX) - 0.5) * 4.0;

                //flyToY = fmax(flyToY, -1.0);

                if (flyToX > 2.0 || flyToY > 2.0) {
                        ROS_ERROR("Cannot fly outside of 2.0/2.0");
                        return;
                }

                Position3D flyToDestination(flyToX, flyToY, FLYTO_Z);
                Position3D flyToLandDestination(flyToX, flyToY, 0.0);
                // Set Destination
                flyTo.getOptionContainer().getOption("tFlyToDestination").set(flyToDestination);
                flyToLand.getOptionContainer().getOption("tLandDestination").set(flyToLandDestination);

                ROS_INFO("Flying to %f,%f", flyToX, flyToY);

                // Note:: TakeOff is GroundState
                if (*activeBehaviorPtr == ground) {
                        bController->switchBehavior(flyToTakeOff);
                } else {
                        bController->switchBehavior(flyTo);
                }
        }

        // Same Button as Hand Neutral
        if (msg->buttons[9]) {
                neutralPosSet = true;

                if (currState == State::Config) {
                        changeState("#readyT#", State::Ready);
                }

        }


        // what to do

}

void Experiment::readSpin()
{
        // know that it's less then 255.
        char buffer[255];
        int recvBytes;
        while(ros::ok()) {
                udp.doSelect();
                if ((recvBytes = udp.recvRaw(buffer,sizeof(buffer))) > 0) {
                        std::string msg(buffer, recvBytes);
                        ROS_INFO_STREAM("Got Message: " << msg);
                        // parse #
                        std::vector<std::string> segments;
                        boost::split(segments, msg, boost::is_any_of("#"));
                        ROS_INFO_STREAM("segments.size(): " << segments.size());


                        std::string command = segments.at(1);

                        ROS_INFO_STREAM("Got command: " << command);



                        // State Machine
                        if (currState == State::Init && command == "readyV") {
                                ROS_INFO("State: init->config");
                                // Get Coodinates and flyto

                                double flyToX = 0.0;
                                double flyToY = 0.0;
                                if (segments.size() > 5) {
                                        flyToX = boost::lexical_cast<double>(segments.at(2));
                                        flyToY = -1.0 * boost::lexical_cast<double>(segments.at(4));
                                } else {
                                        ROS_ERROR("Did not receive enough segments! Flying to 0/0/0");
                                        return;
                                }

                                // remove translation and check
                                if (abs(flyToX - options.tXYOffsetTranslation->getValue()(0)) > options.tMaxSquareAreaSize->getValue()) {
                                        changeState("#error#X Value out of bounds!#", State::Error);
                                        return;
                                }

                                if (abs(flyToY - options.tXYOffsetTranslation->getValue()(1)) > options.tMaxSquareAreaSize->getValue()) {
                                        changeState("#error#Y Value out of bounds!#", State::Error);
                                        return;
                                }

                                Position3D flyToDestination(flyToX, flyToY, FLYTO_Z);
                                Position3D flyToLandDestination(flyToX, flyToY, 0.0);
                                // Set Destination
                                flyTo.getOptionContainer().getOption("tFlyToDestination").set(flyToDestination);
                                flyToLand.getOptionContainer().getOption("tLandDestination").set(flyToLandDestination);

                                bController->switchBehavior(flyToTakeOff);

                                // Quick Config State
                                currState = State::Config;

                        } else if (command == "flyto") {
                                // Get Destination
                                double flyToX = 0.0;
                                double flyToY = 0.0;
                                if (segments.size() > 5) {
                                        // Beware coordinate switch
                                        flyToX = boost::lexical_cast<double>(segments.at(2));
                                        flyToY = -1.0 * boost::lexical_cast<double>(segments.at(4));


                                        // remove translation and check
                                        if (abs(flyToX - options.tXYOffsetTranslation->getValue()(0)) > options.tMaxSquareAreaSize->getValue()) {
                                                changeState("#error#X Value out of bounds!#", State::Error);
                                                return;
                                        }

                                        if (abs(flyToY - options.tXYOffsetTranslation->getValue()(1)) > options.tMaxSquareAreaSize->getValue()) {
                                                changeState("#error#Y Value out of bounds!#", State::Error);
                                                return;
                                        }

                                } else {
                                        ROS_ERROR("Did not receive enough segments! Flying to 0/0/0");
                                        return;
                                }

                                Position3D flyToDestination(flyToX, flyToY, FLYTO_Z);
                                Position3D flyToLandDestination(flyToX, flyToY, 0.0);
                                // Set Destination
                                flyTo.getOptionContainer().getOption("tFlyToDestination").set(flyToDestination);
                                flyToLand.getOptionContainer().getOption("tLandDestination").set(flyToLandDestination);

                                // Note:: TakeOff is GroundState
                                if (currState == State::Ready || currState == State::TakeOff) {
                                        // Standard Case With TakeOff
                                        bController->switchBehavior(flyToTakeOff);
                                        currState = State::Flyto;
                                } else if (currState == State::Flyto // this does not include the Touchdown Phase of Flyto.
                                                || currState == State::User) {
                                        // Disrubtive Case WITHOUT TakeOff
                                        bController->switchBehavior(flyTo);
                                        currState = State::Flyto;
                                } else {
                                        ROS_ERROR("Received FlyTo in invalid State: %s", currState.str());
                                }

                        } else if (currState == State::Ready && command == "target") {
                                ROS_INFO("Received Target. Ready for Takeoff.");
                                currState = State::TakeOff; // TakeOff is Ground State
                        } else if (currState == State::Ready && command == "finished") {
                                ROS_INFO("Experiment finished! Turning off Motors.");

                                MKSingleValuePacket motorState(MKDataDefines::MOTOR_STATE,0);
                                if (!mkInterface->updateMKValue(motorState)) {
                                        ROS_ERROR("Could not get Motorstate!");
                                        return;
                                }

                                if (motorState.value == MotorState::On || motorState.value == MotorState::Init) {
                                        // stop
                                        motorState.value = MotorState::Off;
                                        mkInterface->setMKValue(motorState);
                                }

                        } else {
                                ROS_ERROR("Received Command in Invalid State. Command: %s, State: %s", command.c_str(), currState.str());
                        }

                }

                usleep(10);
        }
}

void Experiment::handJoystickCB(const sensor_msgs::Joy::ConstPtr& msg)
{
        // Liftoff // z maximum
        if (currState == State::TakeOff && *activeBehaviorPtr == ground && msg->axes[2] <= -1.0) {
                bController->switchBehavior(takeOff);

                changeState("#liftoff#", State::User);
        }
}

void Experiment::activeBehaviorChanged(telekyb_interface::Behavior newActiveBehavior)
{
        ROS_INFO("activeBehaviorChanged CALLLED!!!!");
        if (newActiveBehavior == ground) {
                if (currState == State::User) {
                        changeState("#touchdown#", State::Ready);
                } else if (currState == State::Flyto) {
                        changeState("#readyT#", State::Ready);
                }
        } else if (newActiveBehavior == flyToLand || newActiveBehavior == land) {
                // Landing in Flyto
                //currState = State::Land; // Ground follows automatically.
        }

}

void Experiment::changeState(const std::string& command, State newState)
{
        ROS_INFO("Changing Experimental State: %s -> %s", currState.str(), newState.str());

        // only send if you have command
        if (command.size() > 0) {
                udp.doSelect();
                udp.sendRaw(command.c_str(),command.size()+1);
        }

        currState = newState;
}

}