Joystick.cpp

Go to the documentation of this file.

/*
 * Joystick.cpp
 *
 *  Created on: Nov 12, 2011
 *      Author: mriedel
 */

#include "Joystick.hpp"

#include <telekyb_base/ROS.hpp>

// Declare
PLUGINLIB_DECLARE_CLASS(tk_be_common, Joystick, telekyb_behavior::Joystick, TELEKYB_NAMESPACE::Behavior);

namespace telekyb_behavior {

Joystick::Joystick()
        : Behavior("tk_be_common/Joystick", BehaviorType::Air)
{

}

void Joystick::joystickCB(const sensor_msgs::Joy::ConstPtr& msg)
{
//      ROS_INFO("Received Joystick CB");

        if (msg->axes.size() < 4) {
                ROS_ERROR("We need at least 4 Axes!");
                return;
        }

        if (msg->buttons.size() < 2) {
                ROS_ERROR("We need at least 2 Buttons!");
                return;
        }

        //dead man button is pressed, or we do not care about the dead man button
        if ( msg->buttons[0] || !tJoystickUseDeadManSwitch->getValue()) {
                lastVelocityInput = Velocity3D(msg->axes[0],msg->axes[1],msg->axes[2]);
                if (msg->axes.size() != 4) {
                        lastYawRateInput = (msg->axes[4] - msg->axes[3])/2.0 * tJoystickYawRateScale->getValue();
                } else {
                        // 4 axes case
                        lastYawRateInput = msg->axes[3] * tJoystickYawRateScale->getValue();
                }
        } else {
                // apply 0
                lastVelocityInput = Velocity3D::Zero();
                lastYawRateInput = 0.0;
        }


        // invalidate
        if ( msg->buttons[1] ) {
                valid = false;
        }
}

void Joystick::initialize()
{
        tJoystickTopic = addOption<std::string>("tJoystickTopic",
                        "JoystickTopic that published sensor_msgs::Joy",
                        "/TeleKyb/tJoy/joy", false, false);

        tJoystickUsePositionMode = addOption("tJoystickUsePositionMode",
                        "Integrates Position from Velocity input.", true, false, false);
        tJoystickYawRateScale = addOption("tJoystickYawRateScale",
                        "Commanded Yaw Rate is scaled to this value", 1.0, false ,false);

        tJoystickUseRelativeMode = addOption("tJoystickUseRelativeMode",
                        "Enable this to have all inputs interpreted w.r.t. the local frame.", false, false, true);
        tJoystickUseDeadManSwitch = addOption("tJoystickUseDeadManSwitch",
                        "Disable this to fly without pressing a special button simultaneously.", true, false, true);

        nodeHandle = ROSModule::Instance().getMainNodeHandle();

        // no Parameters
        //parameterInitialized = true;
}

void Joystick::destroy()
{

}

bool Joystick::willBecomeActive(const TKState& currentState, const Behavior& previousBehavior)
{
        joySub = nodeHandle.subscribe(tJoystickTopic->getValue()
                        , 10, &Joystick::joystickCB, this);

        lastVelocityInput = Velocity3D::Zero();
        lastYawRateInput = 0.0;

        if (tJoystickUsePositionMode->getValue()) {
                posModeCurPosition = currentState.position;
                posModeCurYawAngle = currentState.getEulerRPY()(2);
                posModeLastInputTime = Time();
        }

        valid = true;

        return true;
}

void Joystick::didBecomeActive(const TKState& currentState, const Behavior& previousBehavior)
{
        // not used
}

void Joystick::willBecomeInActive(const TKState& currentState, const Behavior& nextBehavior)
{
        // not used
}

void Joystick::didBecomeInActive(const TKState& currentState, const Behavior& nextBehavior)
{
        joySub.shutdown();
}

// called everytime a new TKState is available AND it is the NEW Behavior of an active Switch
void Joystick::trajectoryStepCreation(const TKState& currentState, TKTrajectory& generatedTrajInput)
{
        generatedTrajInput.setVelocity(lastVelocityInput);
        generatedTrajInput.setYawRate(lastYawRateInput);
}

// called everytime a new TKState is available. Should return false if invalid (swtich to next behavior, or Hover if undef).
void Joystick::trajectoryStepActive(const TKState& currentState, TKTrajectory& generatedTrajInput)
{
        //      ROS_INFO_STREAM("InputVel: " << std::endl << lastVelocityInput);
        //      ROS_INFO_STREAM("InputYawRate: " << lastYawRateInput);

                // set Velocity Mode
                if (tJoystickUsePositionMode->getValue()) {
                        // integrate Position
                        double timeDiffSec = (Time() - posModeLastInputTime).toDSec();
        //              ROS_INFO("timeDiffSec: %f", timeDiffSec);
                        posModeLastInputTime = Time();
                        if (tJoystickUseRelativeMode->getValue()) {
                                //all is w.r.t. local frame
                                Eigen::Vector2d partial = currentState.orientation.toRotationMatrix().block<2,2>(0,0) * lastVelocityInput.block<2,1>(0,0);
                                Eigen::Vector3d newVel(partial(0), partial(1), lastVelocityInput(2));
                                posModeCurPosition += (newVel * timeDiffSec);
                        } else {
                                //all is w.r.t. world frame
                                posModeCurPosition += (lastVelocityInput * timeDiffSec);
                        }

                        posModeCurYawAngle += (lastYawRateInput * timeDiffSec);

        //              ROS_INFO_STREAM("Position: " << std::endl << posModeCurPosition);
        //              ROS_INFO_STREAM("YawAngle: " << posModeCurYawAngle.dCast());

                        if (tJoystickUseRelativeMode->getValue()) {
                                //all is w.r.t. local frame
                                Eigen::Vector2d partial = currentState.orientation.toRotationMatrix().block<2,2>(0,0) * lastVelocityInput.block<2,1>(0,0);
                                Eigen::Vector3d newVel(partial(0), partial(1), lastVelocityInput(2));
                                generatedTrajInput.setPosition(posModeCurPosition,newVel);
                                generatedTrajInput.setYawAngle(posModeCurYawAngle.dCastPi(0), lastYawRateInput);
                        } else {
                                //all is w.r.t. world frame
                                generatedTrajInput.setPosition(posModeCurPosition, lastVelocityInput);
                                generatedTrajInput.setYawAngle(posModeCurYawAngle.dCastPi(0), lastYawRateInput);
                        }

                } else {
                        if (tJoystickUseRelativeMode->getValue()) {
                                //all is w.r.t. local frame
                                Eigen::Vector2d partial = currentState.orientation.toRotationMatrix().block<2,2>(0,0) * lastVelocityInput.block<2,1>(0,0);
                                Eigen::Vector3d newVel(partial(0), partial(1), lastVelocityInput(2));
                                generatedTrajInput.setVelocity(newVel);
                                generatedTrajInput.setYawRate(lastYawRateInput);
                        } else{
                                //all is w.r.t. world frame
                                generatedTrajInput.setVelocity(lastVelocityInput);
                                generatedTrajInput.setYawRate(lastYawRateInput);
                        }
                }
}

// called everytime a new TKState is available AND it is the OLD Behavior of an active Switch
void Joystick::trajectoryStepTermination(const TKState& currentState, TKTrajectory& generatedTrajInput)
{
        generatedTrajInput.setVelocity( Velocity3D::Zero() );
        generatedTrajInput.setYawRate( 0.0 );
}

// Return true if the active Behavior is (still) valid. Initiate Switch otherwise
bool Joystick::isValid(const TKState& currentState) const
{
        // never turns invalid
        return valid;
}


}