PTUDriver.h

Go to the documentation of this file.

#ifndef PTUDRIVER_H_
#define PTUDRIVER_H_

#include <string.h>
#include <math.h>
#include "ros/ros.h"
#include <limits>
#include "PTUFree.h"
/*
extern "C" {
#include "../../cpi-v1.09.15/include/ptu.h"
}
*/
#define STOPPED_THRESHOLD 0.1


#ifdef __PTU_FREE_INCLUDED__
#define PAN 0
#define TILT 1
#define RESOLUTION 2
#define POSITION_LIMITS_MODE 3
#define OFF_MODE 4
#define BASE 5
#define ABSOLUTE 6
#define UPPER_SPEED_LIMIT 7
#define SPEED 8
#define ACCELERATION 9
#define HOLD_POWER_LEVEL 10
#define MOVE_POWER_LEVEL 11
#define MINIMUM_POSITION 12
#define MAXIMUM_POSITION 13
#define LOWER_SPEED_LIMIT 14
#define POSITION 15
#define PTU_LOW_POWER 16
#define PTU_REG_POWER 17
#define PTU_HI_POWER 18
#define PTU_PURE_VELOCITY_SPEED_CONTROL_MODE 19
#define PTU_OFF_POWER 20
#define SPEED_CONTROL_MODE 21
#define PTU_INDEPENDENT_SPEED_CONTROL_MODE 22
#define PTU_OK 23
#define PTU_NOT_OK 24
#endif


//TODO: Test Methods in pure speed control mode

namespace asr_flir_ptu_driver {
#define HOLZ

class PTUDriver {
    #define HOLZ
    public:
        PTUDriver(const char* port, int baud, bool speed_control);
        PTUDriver();

        ~PTUDriver();


        bool virtual isConnected();

        void virtual setSettings(int pan_base, int tilt_base, int pan_speed, int tilt_speed, int pan_upper, int tilt_upper,
                        int pan_accel, int tilt_accel, int pan_hold, int tilt_hold, int pan_move, int tilt_move);


        void virtual setLimitAngles(double pan_min, double pan_max, double tilt_min, double tilt_max);

        void virtual setAbsoluteAngleSpeeds(double pan_speed, double tilt_speed); //in deg/s

        void virtual setAbsoluteAngleSpeeds(signed short pan_speed, signed short tilt_speed);

        bool virtual setAbsoluteAngles(double pan_angle, double tilt_angle, bool no_forbidden_area_check); //in deg

        bool virtual setValuesOutOfLimitsButWithinMarginToLimit(double * pan, double * tilt, double margin);

        bool virtual isWithinPanTiltLimits(double pan, double tilt);

        void virtual setLimitAnglesToHardwareConstraints();

        void virtual setForbiddenAreas(std::vector< std::map< std::string, double> > forbidden_areas);

        void virtual setSpeedControlMode(bool speed_control_mode);

        bool virtual isInSpeedControlMode();

        double virtual getCurrentAngle(char type);

        double virtual getDesiredAngle(char type);

        double virtual getAngleSpeed(char type);

        bool virtual isInForbiddenArea(double pan_angle, double tilt_angle);

        void virtual setComputationTolerance(double computation_tolerance);

        void virtual setDistanceFactor(long distance_factor);



        long virtual getLimitAngle(char pan_or_tilt, char upper_or_lower);

        bool virtual hasHaltedAndReachedGoal();

        bool virtual hasHalted();

        bool virtual reachedGoal();

        std::vector<double> virtual determineLegitEndPoint(double end_point_pan_candidate, double end_point_tilt_candidate);



   protected:

        std::string virtual getErrorString(char status_code);
        std::vector< std::map< std::string, double> > forbidden_areas;
        long pan_min, pan_max;
        long tilt_min, tilt_max;

        long prefetched_pan_current_base;
        long prefetched_pan_desired_acceleration;
        long prefetched_pan_desired_speed;
        long prefetched_tilt_current_base;
        long prefetched_tilt_desired_acceleration;
        long prefetched_tilt_desired_speed;
        long prefetched_pan_current_position;
        long prefetched_tilt_current_position;
        double pan_resolution, tilt_resolution;
    private:
        void setValuesToBackupValues(int & pan_base, int & tilt_base, int & pan_speed, int & tilt_speed,
                        int & pan_upper, int & tilt_upper, int & pan_accel, int & tilt_accel,
                        int & pan_hold, int & tilt_hold, int & pan_move, int & tilt_move);
        void createSettingsBackup();
        bool checkReturnCode(char return_code);
        void restoreSettingsFromBackup();
        void prefetchValues();

        double double_computation_tolerance;
        bool speed_control;
        static short int POW_VAL_MOVE[3];
        static short int POW_VAL_HOLD[3];
        std::map<std::string,int> backup_settings;
        long convertPanFromAngleToPosition(double angle);
        double convertPanFromPositionToAngle(long position);
        long convertTiltFromAngleToPosition(double angle);
        double convertTiltFromPositionToAngle(long position);
        double pan_acceleration_time;
        double pan_slew_speed_time;
        double tilt_acceleration_time;
        double tilt_slew_speed_time;
        std::vector<std::vector<double> > forbidden_area_first_line_coordinate_forms;
        std::vector<std::vector<double> > forbidden_area_second_line_coordinate_forms;
        std::vector<std::vector<double> > forbidden_area_third_line_coordinate_forms;
        std::vector<std::vector<double> > forbidden_area_fourth_line_coordinate_forms;

        std::vector<std::vector<double> > max_pan_max_tilt_points;
        std::vector<std::vector<double> > max_pan_min_tilt_points;
        std::vector<std::vector<double> > min_pan_max_tilt_points;
        std::vector<std::vector<double> > min_pan_min_tilt_points;

        long backup_pan_base;
        long backup_pan_upper;
        long backup_pan_speed;
        long backup_pan_accel;
        long backup_pan_hold;
        long backup_pan_move;
        long backup_tilt_base;
        long backup_tilt_upper;
        long backup_tilt_speed;
        long backup_tilt_accel;
        long backup_tilt_hold;
        long backup_tilt_move;


        std::vector<double> solveSecondDegreePolynomial(double a, double b, double c);
        std::vector<double> getAccelerationTimeAndSlewSpeedTime(double distance_in_steps, double base_speed, double acceleration, double slew_speed);
        std::vector<double> predictPositionInTime(std::vector<double> start_point, std::vector<double> end_point, double point_in_time);
        double calculateCoveredDistance(double acceleration_time, double slew_speed_time, double decceleration_time, bool is_pan);
        std::vector<double> calculatePointOfIntersectionWithForbiddenAreas(std::vector<double> start_point, std::vector<double> end_point);
        std::vector<double> calculateIntersectionPoint(std::vector<double> first_line_coordiante_form, std::vector<double> second_line_coordiante_form);
        bool isOnLineSegmentBetweenTwoPoints(std::vector<double> start_point, std::vector<double> end_point, std::vector<double> line_coordinate_form, std::vector<double> point_to_check, double tolerance);
        std::vector<double> calculateCoordinateForm(std::vector<double> start_point, std::vector<double> end_point);
        double getVectorLength(std::vector<double> input_vector);
        double getVectorLength(std::vector<double> start_point, std::vector<double> end_point);
        std::vector<double> checkForPossibleKollision(double new_pan_angle, double new_tilt_angle);
        double distance_factor;
        void precalculateForbiddenAreaCoodinateForms();

        #ifndef __PTU_FREE_INCLUDED__
        portstream_fd COMstream;
        #endif

        #ifdef __PTU_FREE_INCLUDED__
        long get_current(char pan_or_tilt, char what);
        char set_desired(char pan_or_tilt, char what, short int * value, char type);
        long get_desired(char pan_or_tilt, char what);
        char set_mode(char mode_type, char mode);
        ptu_free::PTUFree free_ptu;
        #endif




};

}

#endif /* PTU46DRIVER_H_ */