PTUDriver.h
Go to the documentation of this file.
1 
8 #ifndef PTUDRIVER_H_
9 #define PTUDRIVER_H_
10 
11 #include <string.h>
12 #include <math.h>
13 #include "ros/ros.h"
14 #include <limits>
15 #include "PTUFree.h"
16 /*
17 extern "C" {
18 #include "../../cpi-v1.09.15/include/ptu.h"
19 }
20 */
21 #define STOPPED_THRESHOLD 0.1
22 
23 
24 #ifdef __PTU_FREE_INCLUDED__
25 #define PAN 0
26 #define TILT 1
27 #define RESOLUTION 2
28 #define POSITION_LIMITS_MODE 3
29 #define OFF_MODE 4
30 #define BASE 5
31 #define ABSOLUTE 6
32 #define UPPER_SPEED_LIMIT 7
33 #define SPEED 8
34 #define ACCELERATION 9
35 #define HOLD_POWER_LEVEL 10
36 #define MOVE_POWER_LEVEL 11
37 #define MINIMUM_POSITION 12
38 #define MAXIMUM_POSITION 13
39 #define LOWER_SPEED_LIMIT 14
40 #define POSITION 15
41 #define PTU_LOW_POWER 16
42 #define PTU_REG_POWER 17
43 #define PTU_HI_POWER 18
44 #define PTU_PURE_VELOCITY_SPEED_CONTROL_MODE 19
45 #define PTU_OFF_POWER 20
46 #define SPEED_CONTROL_MODE 21
47 #define PTU_INDEPENDENT_SPEED_CONTROL_MODE 22
48 #define PTU_OK 23
49 #define PTU_NOT_OK 24
50 #endif
51 
52 
53 //TODO: Test Methods in pure speed control mode
54 
56 #define HOLZ
57 
58 class PTUDriver {
59  #define HOLZ
60  public:
67  PTUDriver(const char* port, int baud, bool speed_control);
71  PTUDriver();
72 
76  ~PTUDriver();
77 
78 
83  bool virtual isConnected();
84 
100  void virtual setSettings(int pan_base, int tilt_base, int pan_speed, int tilt_speed, int pan_upper, int tilt_upper,
101  int pan_accel, int tilt_accel, int pan_hold, int tilt_hold, int pan_move, int tilt_move);
102 
103 
111  void virtual setLimitAngles(double pan_min, double pan_max, double tilt_min, double tilt_max);
112 
118  void virtual setAbsoluteAngleSpeeds(double pan_speed, double tilt_speed); //in deg/s
119 
125  void virtual setAbsoluteAngleSpeeds(signed short pan_speed, signed short tilt_speed);
126 
135  bool virtual setAbsoluteAngles(double pan_angle, double tilt_angle, bool no_forbidden_area_check); //in deg
136 
144  bool virtual setValuesOutOfLimitsButWithinMarginToLimit(double * pan, double * tilt, double margin);
145 
152  bool virtual isWithinPanTiltLimits(double pan, double tilt);
153 
158 
163  void virtual setForbiddenAreas(std::vector< std::map< std::string, double> > forbidden_areas);
164 
169  void virtual setSpeedControlMode(bool speed_control_mode);
170 
175  bool virtual isInSpeedControlMode();
176 
182  double virtual getCurrentAngle(char type);
183 
189  double virtual getDesiredAngle(char type);
190 
196  double virtual getAngleSpeed(char type);
197 
204  bool virtual isInForbiddenArea(double pan_angle, double tilt_angle);
205 
210  void virtual setComputationTolerance(double computation_tolerance);
211 
217  void virtual setDistanceFactor(long distance_factor);
218 
220 
221 
228  long virtual getLimitAngle(char pan_or_tilt, char upper_or_lower);
229 
234  bool virtual hasHaltedAndReachedGoal();
235 
240  bool virtual hasHalted();
241 
246  bool virtual reachedGoal();
247 
254  std::vector<double> virtual determineLegitEndPoint(double end_point_pan_candidate, double end_point_tilt_candidate);
255 
256 
257 
258  protected:
259 
260  std::string virtual getErrorString(char status_code);
261  std::vector< std::map< std::string, double> > forbidden_areas;
264 
274  private:
275  void setValuesToBackupValues(int & pan_base, int & tilt_base, int & pan_speed, int & tilt_speed,
276  int & pan_upper, int & tilt_upper, int & pan_accel, int & tilt_accel,
277  int & pan_hold, int & tilt_hold, int & pan_move, int & tilt_move);
278  void createSettingsBackup();
279  bool checkReturnCode(char return_code);
281  void prefetchValues();
282 
285  static short int POW_VAL_MOVE[3];
286  static short int POW_VAL_HOLD[3];
287  std::map<std::string,int> backup_settings;
288  long convertPanFromAngleToPosition(double angle);
289  double convertPanFromPositionToAngle(long position);
290  long convertTiltFromAngleToPosition(double angle);
291  double convertTiltFromPositionToAngle(long position);
296  std::vector<std::vector<double> > forbidden_area_first_line_coordinate_forms;
297  std::vector<std::vector<double> > forbidden_area_second_line_coordinate_forms;
298  std::vector<std::vector<double> > forbidden_area_third_line_coordinate_forms;
299  std::vector<std::vector<double> > forbidden_area_fourth_line_coordinate_forms;
300 
301  std::vector<std::vector<double> > max_pan_max_tilt_points;
302  std::vector<std::vector<double> > max_pan_min_tilt_points;
303  std::vector<std::vector<double> > min_pan_max_tilt_points;
304  std::vector<std::vector<double> > min_pan_min_tilt_points;
305 
318 
319 
320  std::vector<double> solveSecondDegreePolynomial(double a, double b, double c);
321  std::vector<double> getAccelerationTimeAndSlewSpeedTime(double distance_in_steps, double base_speed, double acceleration, double slew_speed);
322  std::vector<double> predictPositionInTime(std::vector<double> start_point, std::vector<double> end_point, double point_in_time);
323  double calculateCoveredDistance(double acceleration_time, double slew_speed_time, double decceleration_time, bool is_pan);
324  std::vector<double> calculatePointOfIntersectionWithForbiddenAreas(std::vector<double> start_point, std::vector<double> end_point);
325  std::vector<double> calculateIntersectionPoint(std::vector<double> first_line_coordiante_form, std::vector<double> second_line_coordiante_form);
326  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);
327  std::vector<double> calculateCoordinateForm(std::vector<double> start_point, std::vector<double> end_point);
328  double getVectorLength(std::vector<double> input_vector);
329  double getVectorLength(std::vector<double> start_point, std::vector<double> end_point);
330  std::vector<double> checkForPossibleKollision(double new_pan_angle, double new_tilt_angle);
333 
334  #ifndef __PTU_FREE_INCLUDED__
335  portstream_fd COMstream;
336  #endif
337 
338  #ifdef __PTU_FREE_INCLUDED__
339  long get_current(char pan_or_tilt, char what);
340  char set_desired(char pan_or_tilt, char what, short int * value, char type);
341  long get_desired(char pan_or_tilt, char what);
342  char set_mode(char mode_type, char mode);
344  #endif
345 
346 
347 
348 
349 };
350 
351 }
352 
353 #endif /* PTU46DRIVER_H_ */
virtual void setSpeedControlMode(bool speed_control_mode)
setSpeedControlMode Method to set and disable pure speed control mode
Definition: PTUDriver.cpp:523
virtual bool isConnected()
isConnected Method to check if ptu is connected.
Definition: PTUDriver.cpp:69
double calculateCoveredDistance(double acceleration_time, double slew_speed_time, double decceleration_time, bool is_pan)
Definition: PTUDriver.cpp:840
double getVectorLength(std::vector< double > input_vector)
Definition: PTUDriver.cpp:1007
virtual bool reachedGoal()
reachedGoal Method to determine if the PTU has reached its goal.
Definition: PTUDriver.cpp:653
virtual bool isWithinPanTiltLimits(double pan, double tilt)
isWithinPanTiltLimits Method that checks if pan and tilt value are within the chosen pan and tilt lim...
Definition: PTUDriver.cpp:385
std::vector< std::vector< double > > min_pan_max_tilt_points
Definition: PTUDriver.h:303
virtual double getDesiredAngle(char type)
getDesiredAngle Method to get the desired pan/tilt angle (where the ptu moves to) ...
Definition: PTUDriver.cpp:549
virtual std::vector< double > determineLegitEndPoint(double end_point_pan_candidate, double end_point_tilt_candidate)
determineLegitEndPoint Method to determine an end point which is not inside a forbidden area using pa...
Definition: PTUDriver.cpp:1026
std::vector< double > calculateCoordinateForm(std::vector< double > start_point, std::vector< double > end_point)
Definition: PTUDriver.cpp:992
virtual void setComputationTolerance(double computation_tolerance)
setComputationTolerance Sets the tolerance value for errors due to the imprecise nature of float...
Definition: PTUDriver.cpp:78
long get_current(char pan_or_tilt, char what)
virtual void setDistanceFactor(long distance_factor)
setDistanceFactor Method to set the factor which determines how much samples are going to be taken fo...
Definition: PTUDriver.cpp:82
long get_desired(char pan_or_tilt, char what)
long convertTiltFromAngleToPosition(double angle)
Definition: PTUDriver.cpp:581
virtual bool setAbsoluteAngles(double pan_angle, double tilt_angle, bool no_forbidden_area_check)
setAbsoluteAngles Method to set pan and tilt angle for the PTU
Definition: PTUDriver.cpp:453
virtual bool isInSpeedControlMode()
isInSpeedControlMode Method to determine if ptu is in pure speed control mode
Definition: PTUDriver.cpp:532
double convertPanFromPositionToAngle(long position)
Definition: PTUDriver.cpp:577
virtual bool setValuesOutOfLimitsButWithinMarginToLimit(double *pan, double *tilt, double margin)
setValuesOutOfLimitsButWithinMarginToLimit This method is used to set pan/tilt values that are slighl...
Definition: PTUDriver.cpp:400
virtual long getLimitAngle(char pan_or_tilt, char upper_or_lower)
!!!!!!!!!Müssen nachfolgende public sein? Nach erstellen von mock überprüfen!!!!!!!!!!! ...
Definition: PTUDriver.cpp:278
virtual void 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)
setSettings Method to configure various settings of the ptu
Definition: PTUDriver.cpp:87
std::vector< double > calculatePointOfIntersectionWithForbiddenAreas(std::vector< double > start_point, std::vector< double > end_point)
Definition: PTUDriver.cpp:875
virtual void setLimitAngles(double pan_min, double pan_max, double tilt_min, double tilt_max)
setSettings Method to configure limits for pan and tilt
Definition: PTUDriver.cpp:487
std::vector< std::vector< double > > forbidden_area_third_line_coordinate_forms
Definition: PTUDriver.h:298
ptu_free::PTUFree free_ptu
Definition: PTUDriver.h:343
std::vector< std::vector< double > > max_pan_min_tilt_points
Definition: PTUDriver.h:302
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)
Definition: PTUDriver.cpp:187
std::vector< std::vector< double > > forbidden_area_fourth_line_coordinate_forms
Definition: PTUDriver.h:299
~PTUDriver()
~PTUDriver Do not use, necessary because of some compiler issues.
Definition: PTUDriver.cpp:64
static short int POW_VAL_MOVE[3]
Definition: PTUDriver.h:285
std::vector< std::vector< double > > forbidden_area_first_line_coordinate_forms
Definition: PTUDriver.h:296
std::map< std::string, int > backup_settings
Definition: PTUDriver.h:287
virtual std::string getErrorString(char status_code)
Definition: PTUDriver.cpp:589
virtual void setForbiddenAreas(std::vector< std::map< std::string, double > > forbidden_areas)
setForbiddenAreas Method to set all forbidden areas for the PTU. Former forbidden areas will be disca...
Definition: PTUDriver.cpp:308
std::vector< double > getAccelerationTimeAndSlewSpeedTime(double distance_in_steps, double base_speed, double acceleration, double slew_speed)
Definition: PTUDriver.cpp:687
bool checkReturnCode(char return_code)
Definition: PTUDriver.cpp:257
virtual double getAngleSpeed(char type)
getAngleSpeed Method to get the current angle speed of an axisa (deg/s)
Definition: PTUDriver.cpp:561
PTUDriver()
PTUDriver Do not use, needed for mock.
Definition: PTUDriver.cpp:60
virtual double getCurrentAngle(char type)
getCurrentAngle Method to get the current pan/tilt angle
Definition: PTUDriver.cpp:536
std::vector< std::vector< double > > forbidden_area_second_line_coordinate_forms
Definition: PTUDriver.h:297
static short int POW_VAL_HOLD[3]
Definition: PTUDriver.h:286
virtual bool hasHaltedAndReachedGoal()
hasHaltedAndReachedGoal Method to determine if the PTU has halted and reached its goal ...
Definition: PTUDriver.cpp:642
virtual void setLimitAnglesToHardwareConstraints()
setLimitAnglesToHardwareConstraints Method that sets the pan and tilt limits to the maximum values th...
Definition: PTUDriver.cpp:443
std::vector< std::vector< double > > min_pan_min_tilt_points
Definition: PTUDriver.h:304
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)
Definition: PTUDriver.cpp:950
double convertTiltFromPositionToAngle(long position)
Definition: PTUDriver.cpp:585
std::vector< double > checkForPossibleKollision(double new_pan_angle, double new_tilt_angle)
Definition: PTUDriver.cpp:1053
std::vector< double > solveSecondDegreePolynomial(double a, double b, double c)
Definition: PTUDriver.cpp:666
virtual bool isInForbiddenArea(double pan_angle, double tilt_angle)
isInForbiddenArea method to determine if a pan + tilt value pair lies within a forbidden area...
Definition: PTUDriver.cpp:362
char set_desired(char pan_or_tilt, char what, short int *value, char type)
long convertPanFromAngleToPosition(double angle)
Definition: PTUDriver.cpp:573
std::vector< std::map< std::string, double > > forbidden_areas
Definition: PTUDriver.h:261
std::vector< double > calculateIntersectionPoint(std::vector< double > first_line_coordiante_form, std::vector< double > second_line_coordiante_form)
Definition: PTUDriver.cpp:934
virtual void setAbsoluteAngleSpeeds(double pan_speed, double tilt_speed)
setAbsoluteAngleSpeeds Method to set desired speed for pan/tilt movement absolute in deg/s ...
Definition: PTUDriver.cpp:331
virtual bool hasHalted()
hasHalted Method to determine if PTU movement has stopped
Definition: PTUDriver.cpp:646
std::vector< double > predictPositionInTime(std::vector< double > start_point, std::vector< double > end_point, double point_in_time)
Definition: PTUDriver.cpp:736
std::vector< std::vector< double > > max_pan_max_tilt_points
Definition: PTUDriver.h:301
char set_mode(char mode_type, char mode)


asr_flir_ptu_driver
Author(s): Valerij Wittenbeck, Joachim Gehrung, Pascal Meißner, Patrick Schlosser
autogenerated on Mon Feb 28 2022 21:41:05