StandardTrajectoryTracker.hpp

Go to the documentation of this file.

/*
 * StandardTrajectoryTracker.hpp
 *
 *  Created on: Nov 8, 2011
 *      Author: mriedel
 */

#ifndef STANDARDTRAJECTORYTRACKER_HPP_
#define STANDARDTRAJECTORYTRACKER_HPP_

#include <telekyb_defines/telekyb_defines.hpp>


// Interface Definition
#include <tk_trajctrl/TrajectoryTracker.hpp>

// Position Controller
#include <tk_ctrlalgo/PositionControl.hpp>
// Yaw Controller
#include <tk_ctrlalgo/YawControl.hpp>
// Dynamic Mass Estimator
// Class Loading
#include <pluginlib/class_loader.h>
// Interface Definition
#include <tk_param_estimator/MassEstimator.hpp>

// Ros
#include <ros/ros.h>


// Boost
#include <boost/thread/mutex.hpp>

namespace TELEKYB_NAMESPACE {


// Option Definition
class StandardTrajectoryTrackerOptions : public OptionContainer {
public:
        //Option<bool>* tCompletelyDisableME;

        Option<std::string>* tCommandsTopic;
        Option<std::string>* tPluginLookupName;

        StandardTrajectoryTrackerOptions();
};

/*
 * OptionListener<bool> -> if MassEstimation is completely disabled!
 */

class StandardTrajectoryTracker : public TrajectoryTracker {
protected:
        // Mass Estimation Option
        Option<bool>* tDoMassEstimation;

        // ClassLoader
        pluginlib::ClassLoader<MassEstimator> meLoader;

        // Loaded Mass Estimator
        MassEstimator* massEstimator;

        StandardTrajectoryTrackerOptions options;
        PositionControl positionControl;
        YawControl yawControl;
        //MassEstimation massEstimation;

        // currentMass -> either constant or estimated by MassEstimator
        double currentMass;

        boost::mutex currentInputMutex;
        TKTrajectory currentInput;

        // Nodehandle
        ros::NodeHandle nodeHandle;
        ros::NodeHandle commandNodeHandle;

        // Publish TKLLCommands
        ros::Publisher tTcCommandsPub;


public:
        StandardTrajectoryTracker();
        virtual ~StandardTrajectoryTracker();

        // Standard Interface functions
        void initialize();
        void destroy();

        std::string getName() const;

        // Callback Functions
        void trajectoryCB(const TKTrajectory& trajectory);
        void stateCB(const TKState& state);


};

}

#endif /* STANDARDTRAJECTORYTRACKER_HPP_ */