ControlNode.h

Go to the documentation of this file.

#pragma once

#ifndef __CONTROLNODE_H
#define __CONTROLNODE_H

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "uga_tum_ardrone/filter_state.h"
#include "std_msgs/String.h"
#include <dynamic_reconfigure/server.h>
#include "uga_tum_ardrone/AutopilotParamsConfig.h"
#include "DroneController.h"
#include "std_msgs/Empty.h"
#include "std_srvs/Empty.h"

#include "uga_tum_ardrone/SetReference.h"
#include "uga_tum_ardrone/SetMaxControl.h"
#include "uga_tum_ardrone/SetInitialReachDistance.h"
#include "uga_tum_ardrone/SetStayWithinDistance.h"
#include "uga_tum_ardrone/SetStayTime.h"
#include "std_srvs/Empty.h"
#include <fstream>

class DroneKalmanFilter;
class MapView;
class PTAMWrapper;
class KIProcedure;


struct ControlNode
{
private:
        ros::Subscriber dronepose_sub;
        ros::Publisher vel_pub;
        ros::Subscriber uga_tum_ardrone_sub;
        ros::Publisher uga_tum_ardrone_pub;
        ros::Publisher takeoff_pub;
        ros::Publisher land_pub;
        ros::Publisher toggleState_pub;

        ros::NodeHandle nh_;
        static pthread_mutex_t uga_tum_ardrone_CS;

        // parameters
        int minPublishFreq;
        std::string control_channel;
        std::string dronepose_channel;
        std::string command_channel;
        std::string packagePath;
        std::string land_channel;
        std::string takeoff_channel;
        std::string toggleState_channel;

        // services
        ros::ServiceServer setReference_;
        ros::ServiceServer setMaxControl_;
        ros::ServiceServer setInitialReachDistance_;
        ros::ServiceServer setStayWithinDistance_;
        ros::ServiceServer setStayTime_;
        ros::ServiceServer startControl_;
        ros::ServiceServer stopControl_;
        ros::ServiceServer clearCommands_;
        ros::ServiceServer hover_;
        ros::ServiceServer lockScaleFP_;

        bool setReference(uga_tum_ardrone::SetReference::Request&, uga_tum_ardrone::SetReference::Response&);
        bool setMaxControl(uga_tum_ardrone::SetMaxControl::Request&, uga_tum_ardrone::SetMaxControl::Response&);
        bool setInitialReachDistance(uga_tum_ardrone::SetInitialReachDistance::Request&, uga_tum_ardrone::SetInitialReachDistance::Response&);
        bool setStayWithinDistance(uga_tum_ardrone::SetStayWithinDistance::Request&, uga_tum_ardrone::SetStayWithinDistance::Response&);
        bool setStayTime(uga_tum_ardrone::SetStayTime::Request&, uga_tum_ardrone::SetStayTime::Response&);
        bool start(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
        bool stop(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
        bool clear(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
        bool hover(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
        bool lockScaleFP(std_srvs::Empty::Request&, std_srvs::Empty::Response&);

        // command queue & KI stuff
        std::deque<std::string> commandQueue;
        static pthread_mutex_t commandQueue_CS;
        // this KI is currently responsible for setting the target etc.
        // if it is "Done", it is set to NULL,
        // if it is NULL, the next command will be popped and parsed from commandQueueu.
        KIProcedure* currentKI;

        // command parameters
        DronePosition parameter_referenceZero;
        double parameter_StayTime;
        double parameter_MaxControl;
        double parameter_InitialReachDist;
        double parameter_StayWithinDist;

        // functions
        void startControl();
        void stopControl();
        void clearCommands();
        void updateControl(const uga_tum_ardrone::filter_stateConstPtr statePtr);

        void popNextCommand(const uga_tum_ardrone::filter_stateConstPtr statePtr);
        void reSendInfo();
        char buf[500];
        ControlCommand lastSentControl;
public:
        ControlNode();
        ~ControlNode();


        // ROS message callbacks
        void droneposeCb(const uga_tum_ardrone::filter_stateConstPtr statePtr);
        void comCb(const std_msgs::StringConstPtr str);
        void dynConfCb(uga_tum_ardrone::AutopilotParamsConfig &config, uint32_t level);

        // main pose-estimation loop
        void Loop();

        // writes a string message to "/uga_tum_ardrone/com".
        // is thread-safe (can be called by any thread, but may block till other calling thread finishes)
        void publishCommand(std::string c);

        // control drone functions
        void sendControlToDrone(ControlCommand cmd);
        void sendLand();
        void sendTakeoff();
        void sendToggleState();

        // controller
        DroneController controller;
        ControlCommand hoverCommand;

        // logging stuff
        std::ofstream* logfileControl;
        static pthread_mutex_t logControl_CS;
        long currentLogID;
        long startedLogClock;
        void toogleLogging();   // switches logging on or off.

        // other internals
        long lastControlSentMS;
        bool isControlling;
};
#endif /* __CONTROLNODE_H */