Fawkes API  Fawkes Development Version
com_thread.h
1 /***************************************************************************
2  * com_thread.h - Robotino com thread base class
3  *
4  * Created: Thu Sep 11 11:43:42 2014
5  * Copyright 2011-2016 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #ifndef __PLUGINS_ROBOTINO_COM_THREAD_H_
22 #define __PLUGINS_ROBOTINO_COM_THREAD_H_
23 
24 #include <core/threading/thread.h>
25 #include <aspect/logging.h>
26 #include <aspect/clock.h>
27 
28 #include <utils/time/time.h>
29 
30 #include <cstdio>
31 
32 namespace fawkes {
33  class Mutex;
34  class Clock;
35 }
36 
37 #define NUM_IR_SENSORS 9
38 
40 : public fawkes::Thread,
41  public fawkes::ClockAspect,
43 {
44  public:
45  struct SensorData {
46  SensorData();
47 
48  /// @cond INTERNAL
49  unsigned int seq;
50 
51  float mot_velocity[3];
52  int32_t mot_position[3];
53  float mot_current[3];
54  bool bumper;
55  bool bumper_estop_enabled;
56  bool digital_in[8];
57  bool digital_out[8];
58  float analog_in[8];
59 
60  float bat_voltage;
61  float bat_current;
62  float bat_absolute_soc;
63 
64  bool imu_enabled;
65  float imu_orientation[4];
66  float imu_angular_velocity[3];
67  double imu_angular_velocity_covariance[9];
68 
69  float odo_x;
70  float odo_y;
71  float odo_phi;
72 
73  float ir_voltages[NUM_IR_SENSORS];
74 
75  fawkes::Time time;
76  /// @endcond
77  };
78 
79  RobotinoComThread(const char *thread_name);
80  virtual ~RobotinoComThread();
81 
82  virtual bool is_connected() = 0;
83 
84  virtual void set_gripper(bool opened) = 0;
85  virtual bool is_gripper_open() = 0;
86  virtual void set_speed_points(float s1, float s2, float s3) = 0;
87  virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t) = 0;
88  virtual void get_odometry(double &x, double &y, double &phi) = 0;
89 
90  virtual void reset_odometry() = 0;
91  virtual void set_bumper_estop_enabled(bool enabled) = 0;
92  virtual void set_motor_accel_limits(float min_accel, float max_accel) = 0;
93  virtual void set_digital_output(unsigned int digital_out, bool enable) = 0;
94 
95  virtual bool get_data(SensorData &sensor_data);
96 
97  void set_drive_layout(float rb, float rw, float gear);
98  void set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel);
99  virtual void set_desired_vel(float vx, float vy, float omega);
100 
101 
102  void project(float *m1, float *m2, float *m3, float vx, float vy, float omega) const;
103  void unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const;
104 
105  protected:
106  bool update_velocities();
107 
108  private:
109 
110  float update_speed(float des, float set, float accel, float decel, float diff_sec);
111 
112  protected:
113  /** Mutex to protect data_. Lock whenever accessing it. */
115  /** Data struct that must be updated whenever new data is available. */
117  /** Flag to indicate new data, set to true if data_ is modified. */
118  bool new_data_;
119 
120  private:
121  float cfg_rb_;
122  float cfg_rw_;
123  float cfg_gear_;
124  float cfg_trans_accel_;
125  float cfg_trans_decel_;
126  float cfg_rot_accel_;
127  float cfg_rot_decel_;
128 
129  fawkes::Mutex *vel_mutex_;
130  fawkes::Time *vel_last_update_;
131  bool vel_last_zero_;
132  float des_vx_;
133  float des_vy_;
134  float des_omega_;
135 
136  float set_vx_;
137  float set_vy_;
138  float set_omega_;
139 
140 #ifdef USE_VELOCITY_RECORDING
141  FILE *f_;
142  fawkes::Time *start_;
143 #endif
144 };
145 
146 
147 #endif
148 
SensorData data_
Data struct that must be updated whenever new data is available.
Definition: com_thread.h:116
Struct to exchange data between com and sensor thread.
Definition: com_thread.h:45
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
Definition: com_thread.h:118
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
Fawkes library namespace.
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
fawkes::Mutex * data_mutex_
Mutex to protect data_.
Definition: com_thread.h:114
Virtual base class for thread that communicates with a Robotino.
Definition: com_thread.h:39
Thread aspect to log output.
Definition: logging.h:35
Mutex mutual exclusion lock.
Definition: mutex.h:32