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
31namespace fawkes {
32class Mutex;
33class Clock;
34} // namespace fawkes
35
36#define NUM_IR_SENSORS 9
37
41{
42public:
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
104protected:
105 bool update_velocities();
106
107private:
108 float update_speed(float des, float set, float accel, float decel, float diff_sec);
109
110protected:
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. */
117
118private:
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
Virtual base class for thread that communicates with a Robotino.
Definition: com_thread.h:41
virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)=0
Get actual velocity.
virtual void reset_odometry()=0
Reset odometry to zero.
virtual void set_gripper(bool opened)=0
Open or close gripper.
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.
virtual bool is_connected()=0
Check if we are connected to OpenRobotino.
virtual void set_bumper_estop_enabled(bool enabled)=0
Enable or disable emergency stop on bumper contact.
virtual void set_speed_points(float s1, float s2, float s3)=0
Set speed points for wheels.
virtual void set_motor_accel_limits(float min_accel, float max_accel)=0
Set acceleration limits of motors.
fawkes::Mutex * data_mutex_
Mutex to protect data_.
Definition: com_thread.h:112
void set_drive_layout(float rb, float rw, float gear)
Set omni drive layout parameters.
Definition: com_thread.cpp:183
virtual bool is_gripper_open()=0
Check if gripper is open.
RobotinoComThread(const char *thread_name)
Constructor.
Definition: com_thread.cpp:114
virtual void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
Definition: com_thread.cpp:214
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 bool get_data(SensorData &sensor_data)
Get all current sensor data.
Definition: com_thread.cpp:165
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 ~RobotinoComThread()
Destructor.
Definition: com_thread.cpp:147
bool update_velocities()
Update velocity values.
Definition: com_thread.cpp:226
SensorData data_
Data struct that must be updated whenever new data is available.
Definition: com_thread.h:114
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
Definition: com_thread.h:116
virtual void set_digital_output(unsigned int digital_out, bool enable)=0
Set digital output state.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to log output.
Definition: logging.h:33
Mutex mutual exclusion lock.
Definition: mutex.h:33
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Fawkes library namespace.
Struct to exchange data between com and sensor thread.
Definition: com_thread.h:44