HandJoystick.cpp

Go to the documentation of this file.

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

#include "HandJoystick.hpp"

#include <telekyb_base/ROS.hpp>

// Declare
PLUGINLIB_DECLARE_CLASS(tk_vrqc2011, HandJoystick, telekyb_behavior::HandJoystick, TELEKYB_NAMESPACE::Behavior);

namespace telekyb_behavior {

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

}

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

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

        // INPUT is already correctly in NED!
        lastVelocityInput = Velocity3D(msg->axes[0],msg->axes[1], msg->axes[2]) * tHandJoystickScale->getValue();
}

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

        tHandJoystickUsePositionMode = addOption("tHandJoystickUsePositionMode",
                        "Integrates Position from Velocity input.", false, false, false);
        //tHandJoystickYawRateScale = addOption("tHandJoystickYawRateScale","Commanded Yaw Rate is scaled to this value", 1.0, false ,false);
        tHandJoystickMinHeight = addOption<double>("tHandJoystickMinHeight",
                        "minimal valid height (z inverse!)", -0.5, false, false);
        tHandJoystickScale = addOption<double>("tHandJoystickScale",
                        "Scale Input by Value", 0.5, false, false);
        nodeHandle = ROSModule::Instance().getMainNodeHandle();

        // no Parameters
        //parameterInitialized = true;
}

void HandJoystick::destroy()
{

}


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

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

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

        //isValid = true;

        return true;
}

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

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

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

// called everytime a new TKState is available AND it is the NEW Behavior of an active Switch
void HandJoystick::trajectoryStepCreation(const TKState& currentState, TKTrajInput& generatedTrajInput)
{
        trajectoryStepActive(currentState, generatedTrajInput);
}

// called everytime a new TKState is available. Should return false if invalid (swtich to next behavior, or Hover if undef).
void HandJoystick::trajectoryStepActive(const TKState& currentState, TKTrajInput& generatedTrajInput)
{
        if (tHandJoystickUsePositionMode->getValue()) {
                // integrate Position
                double timeDiffSec = (Time() - posModeLastInputTime).toDSec();
//              ROS_INFO("timeDiffSec: %f", timeDiffSec);
                posModeLastInputTime = Time();
                posModeCurPosition += (lastVelocityInput * timeDiffSec);
                //posModeCurYawAngle += (0.0 * timeDiffSec);

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

                generatedTrajInput.setPosition(posModeCurPosition, lastVelocityInput);
                generatedTrajInput.setYawAngle(posModeCurYawAngle.dCastPi(0), 0.0);

        } else {
                generatedTrajInput.setVelocity(lastVelocityInput);
                generatedTrajInput.setYawRate(0.0);
        }
}

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

// Return true if the active Behavior is (still) valid. Initiate Switch otherwise
bool HandJoystick::isValid(const TKState& currentState) const
{
        return tHandJoystickMinHeight->getValue() > currentState.position(2);
}

}