Experiment_old.cpp

Go to the documentation of this file.

/*
 * Experiment.cpp
 *
 *  Created on: Oct 26, 2011
 *      Author: mriedel
 */

#include "Experiment.hpp"

#include <telekyb_base/ROS.hpp>

#include <boost/algorithm/string.hpp>

namespace telekyb {

Experiment::Experiment()
        : currState(State::Init), options(ExperimentOptions::InstancePtr()), setNextNeutralPosition(false)
{
        nodeHandle = ROSModule::Instance().getMainNodeHandle();
        joySub = nodeHandle.subscribe(options->tJoystickTopic->getValue()
                        , 10, &Experiment::joystickCB, this);
        inputTransformSub = nodeHandle.subscribe(options->tTransformInTopic->getValue()
                        , 1, &Experiment::inputCB, this);
        inputTransformPub = nodeHandle.advertise<geometry_msgs::TransformStamped>(options->tTransformOutTopic->getValue(),1);
        udp.initSender(options->tClientHostname->getValue().c_str(), options->tClientPort->getValue());
        udp.initReceiver(options->tClientPort->getValue());


        neutralPosition = Position3D::Zero();
        maxInputOffset = options->tMaxInputOffset->getValue();

}

Experiment::~Experiment()
{

}

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");
                                currState = State::Config;
                        } else if (currState == State::Ready && command == "target") {
                                ROS_INFO("Got target!");
                                //currState = State::Liftoff;
                        } else if (currState == State::Touchdown && command == "flyto") {
                                ROS_INFO("Got Flyto!");
                                currState = State::Flyto;
                        } else if (currState == State::User && command == "flyto") {
                                ROS_INFO("Got Flyto!");
                                currState = State::Flyto;
                        }

                }

                usleep(10);
        }
}

void Experiment::joystickCB(const sensor_msgs::Joy::ConstPtr& msg)
{
        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;
        if (msg->buttons.size() > 1 && msg->buttons.at(3)) {
                // Button 1. -> Test emergency
                // send error. sleep send recovered
                ROS_INFO("Button 3 pressed");
                ROS_INFO("Sending Error!");
                command = "#error#Something went wrong!!#";
                udp.doSelect();
                udp.sendRaw(command.c_str(),command.size()+1);
                // sleep 10
                ROS_INFO("Sleeping 10!");
                sleep(10);
                command = "#recovered#";
                udp.doSelect();
                udp.sendRaw(command.c_str(),command.size()+1);
                ROS_INFO("Recovered");

        } else if (msg->buttons.size() > 0 && msg->buttons.at(0)) {
                // Button 0. -> Test cycle
                ROS_INFO("Button 0 pressed");
                // State Machine
                if (currState == State::Init) {
                        command = "#startExp#";
                        udp.doSelect();
                        udp.sendRaw(command.c_str(),command.size()+1);
                        ROS_INFO("Send startExp");
                } else if (currState == State::Config) {
                        command = "#readyT#";
                        udp.doSelect();
                        udp.sendRaw(command.c_str(),command.size()+1);
                        ROS_INFO("State: config->ready");
                        currState = State::Ready;
                } else if (currState == State::Ready) {
                        command = "#liftoff#";
                        udp.doSelect();
                        udp.sendRaw(command.c_str(),command.size()+1);
                        ROS_INFO("State: ready->liftoff");
                        currState = State::Liftoff;
                } else if (currState == State::Liftoff) {
                        ROS_INFO("State: liftoff->user");
                        currState = State::User;
                } else if (currState == State::User) {
                        ROS_INFO("State: user->touchdown");
                        command = "#touchdown#";
                        udp.doSelect();
                        udp.sendRaw(command.c_str(),command.size()+1);
                        currState = State::Touchdown;
                } else if (currState == State::Touchdown) {
                        ROS_INFO("State: touchdown->ready");
                        currState = State::Ready;
                } else if (currState == State::Flyto) {
                        command = "#readyT#";
                        udp.doSelect();
                        udp.sendRaw(command.c_str(),command.size()+1);
                        ROS_INFO("State: flyto->ready");
                        currState = State::Ready;
                }

        } else if (msg->buttons.size() > 0 && msg->buttons.at(1)) {
                ROS_INFO("Button 1 pressed");
                // reset once to current position
                setNextNeutralPosition = true;

        } else {
                // do nothing
        }
}

void Experiment::inputCB(const geometry_msgs::TransformStamped::Ptr& msg)
{
        if (currState == State::Init) {
                // to not send anything
                return;
        }

        // get input. -> transform resend
        Position3D transform;
        transform << msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z;

        if (setNextNeutralPosition) {
                neutralPosition = transform;
                setNextNeutralPosition = false;
        }

        if (neutralPosition == Position3D::Zero()) {
                return;
        }

        //transform
        transform -= neutralPosition;
        transform(0) = -transform(0);
        transform(1) = -transform(1);

        //scale
        transform = transform / maxInputOffset;

        // 1.0 is max

        msg->transform.translation.x = std::min(transform(0),1.0);
        msg->transform.translation.y = std::min(transform(1),1.0);
        msg->transform.translation.z = std::min(transform(2),1.0);

        msg->transform.translation.x = std::max(transform(0),-1.0);
        msg->transform.translation.y = std::max(transform(1),-1.0);
        msg->transform.translation.z = std::max(transform(2),-1.0);


        // send
        inputTransformPub.publish(msg);
}



// State function
void Experiment::init()
{
//      ROS_INFO("currState: %s, sizeof: %d, strlen+1: %d",
//                      currState.str(), sizeof(currState.str()), strlen(currState.str())+1);

        // send startExp
        //SendCmds::startExp.optional_value();
//      SendCmds cmd = SendCmds::startExp;
//      std::string msg = cmd.value();
        //std::cout << "StartExpString: " <<  msg << std::endl;

        //SendCmds::optional_value


        // wait for readyV
}

void Experiment::ready()
{

}
void Experiment::liftoff()
{

}
void Experiment::user()
{

}
void Experiment::touchdown()
{

}
void Experiment::flyto()
{
        // send liftoff

        sleep(10);
        // send touchdown

}

void Experiment::error()
{

}


}