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 <aspect/clock.h>
25 #include <aspect/logging.h>
26 #include <core/threading/thread.h>
27 #include <utils/time/time.h>
28 
29 #include <cstdio>
30 
31 namespace fawkes {
32 class Mutex;
33 class Clock;
34 } // namespace fawkes
35 
36 #define NUM_IR_SENSORS 9
37 
39  public fawkes::ClockAspect,
41 {
42 public:
43  struct SensorData
44  {
45  SensorData();
46 
47  /// @cond INTERNAL
48  unsigned int seq;
49 
50  float mot_velocity[3];
51  int32_t mot_position[3];
52  float mot_current[3];
53  bool bumper;
54  bool bumper_estop_enabled;
55  bool digital_in[8];
56  bool digital_out[8];
57  float analog_in[8];
58 
59  float bat_voltage;
60  float bat_current;
61  float bat_absolute_soc;
62 
63  bool imu_enabled;
64  float imu_orientation[4];
65  float imu_angular_velocity[3];
66  double imu_angular_velocity_covariance[9];
67 
68  float odo_x;
69  float odo_y;
70  float odo_phi;
71 
72  float ir_voltages[NUM_IR_SENSORS];
73 
74  fawkes::Time time;
75  /// @endcond
76  };
77 
78  RobotinoComThread(const char *thread_name);
79  virtual ~RobotinoComThread();
80 
81  virtual bool is_connected() = 0;
82 
83  virtual void set_gripper(bool opened) = 0;
84  virtual bool is_gripper_open() = 0;
85  virtual void set_speed_points(float s1, float s2, float s3) = 0;
86  virtual void
87  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  void project(float *m1, float *m2, float *m3, float vx, float vy, float omega) const;
102  void unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const;
103 
104 protected:
105  bool update_velocities();
106 
107 private:
108  float update_speed(float des, float set, float accel, float decel, float diff_sec);
109 
110 protected:
111  /** Mutex to protect data_. Lock whenever accessing it. */
113  /** Data struct that must be updated whenever new data is available. */
115  /** Flag to indicate new data, set to true if data_ is modified. */
116  bool new_data_;
117 
118 private:
119  float cfg_rb_;
120  float cfg_rw_;
121  float cfg_gear_;
122  float cfg_trans_accel_;
123  float cfg_trans_decel_;
124  float cfg_rot_accel_;
125  float cfg_rot_decel_;
126 
127  fawkes::Mutex *vel_mutex_;
128  fawkes::Time * vel_last_update_;
129  bool vel_last_zero_;
130  float des_vx_;
131  float des_vy_;
132  float des_omega_;
133 
134  float set_vx_;
135  float set_vy_;
136  float set_omega_;
137 
138 #ifdef USE_VELOCITY_RECORDING
139  FILE * f_;
140  fawkes::Time *start_;
141 #endif
142 };
143 
144 #endif
SensorData data_
Data struct that must be updated whenever new data is available.
Definition: com_thread.h:114
virtual ~RobotinoComThread()
Destructor.
Definition: com_thread.cpp:147
void unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const
Project single motor speeds to velocity in cartesian coordinates.
Definition: com_thread.cpp:394
virtual void set_speed_points(float s1, float s2, float s3)=0
Set speed points for wheels.
Struct to exchange data between com and sensor thread.
Definition: com_thread.h:43
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
Definition: com_thread.h:116
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:33
Fawkes library namespace.
virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)=0
Get actual velocity.
bool update_velocities()
Update velocity values.
Definition: com_thread.cpp:226
virtual void reset_odometry()=0
Reset odometry to zero.
A class for handling time.
Definition: time.h:92
virtual void set_motor_accel_limits(float min_accel, float max_accel)=0
Set acceleration limits of motors.
Thread class encapsulation of pthreads.
Definition: thread.h:45
virtual void set_digital_output(unsigned int digital_out, bool enable)=0
Set digital output state.
fawkes::Mutex * data_mutex_
Mutex to protect data_.
Definition: com_thread.h:112
virtual void set_gripper(bool opened)=0
Open or close gripper.
virtual void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
Definition: com_thread.cpp:214
virtual bool is_gripper_open()=0
Check if gripper is open.
virtual void set_bumper_estop_enabled(bool enabled)=0
Enable or disable emergency stop on bumper contact.
void set_drive_layout(float rb, float rw, float gear)
Set omni drive layout parameters.
Definition: com_thread.cpp:183
void project(float *m1, float *m2, float *m3, float vx, float vy, float omega) const
Project the velocity of the robot in cartesian coordinates to single motor speeds.
Definition: com_thread.cpp:361
Virtual base class for thread that communicates with a Robotino.
Definition: com_thread.h:38
Thread aspect to log output.
Definition: logging.h:32
virtual bool get_data(SensorData &sensor_data)
Get all current sensor data.
Definition: com_thread.cpp:165
virtual bool is_connected()=0
Check if we are connected to OpenRobotino.
RobotinoComThread(const char *thread_name)
Constructor.
Definition: com_thread.cpp:114
void set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel)
Set the omni drive limits.
Definition: com_thread.cpp:197
virtual void get_odometry(double &x, double &y, double &phi)=0
Get latest odometry value.
Mutex mutual exclusion lock.
Definition: mutex.h:32