Fawkes API Fawkes Development Version
com_thread.cpp
1
2/***************************************************************************
3 * com_thread.cpp - Robotino com thread base class
4 *
5 * Created: Thu Sep 11 13:18:00 2014
6 * Copyright 2011-2016 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "com_thread.h"
23
24#include <core/threading/mutex.h>
25#include <core/threading/mutex_locker.h>
26
27using namespace fawkes;
28
29/** @class RobotinoComThread "com_thread.h"
30 * Virtual base class for thread that communicates with a Robotino.
31 * A communication thread is always continuous and must communicate at the
32 * required pace. It provides hook for sensor and act threads.
33 * @author Tim Niemueller
34 *
35 *
36 * @fn void RobotinoComThread::reset_odometry() = 0
37 * Reset odometry to zero.
38 *
39 * @fn bool RobotinoComThread::is_connected() = 0
40 * Check if we are connected to OpenRobotino.
41 * @return true if the connection has been established, false otherwise
42 *
43 * @fn void RobotinoComThread::get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t) = 0
44 * Get actual velocity.
45 * @param a1 upon return contains velocity in RPM for first wheel
46 * @param a2 upon return contains velocity in RPM for second wheel
47 * @param a3 upon return contains velocity in RPM for third wheel
48 * @param seq upon return contains sequence number of latest data
49 * @param t upon return contains time of latest data
50 *
51 * @fn bool RobotinoComThread::is_gripper_open() = 0
52 * Check if gripper is open.
53 * @return true if the gripper is presumably open, false otherwise
54 *
55 * @fn void RobotinoComThread::set_speed_points(float s1, float s2, float s3) = 0
56 * Set speed points for wheels.
57 * @param s1 speed point for first wheel in RPM
58 * @param s2 speed point for second wheel in RPM
59 * @param s3 speed point for third wheel in RPM
60 *
61 * @fn void RobotinoComThread::set_gripper(bool opened) = 0
62 * Open or close gripper.
63 * @param opened true to open gripper, false to close
64 *
65 * @fn void RobotinoComThread::get_odometry(double &x, double &y, double &phi) = 0
66 * Get latest odometry value.
67 * @param x upon return contains x coordinate of odometry
68 * @param y upon return contains y coordinate of odometry
69 * @param phi upon return contains rptation of odometry
70 *
71 * @fn void RobotinoComThread::set_bumper_estop_enabled(bool enabled) = 0
72 * Enable or disable emergency stop on bumper contact.
73 * @param enabled true to enable, false to disable
74 *
75 * @fn void RobotinoComThread::set_motor_accel_limits(float min_accel, float max_accel) = 0
76 * Set acceleration limits of motors.
77 * @param min_accel minimum acceleration
78 * @param max_accel maximum acceleration
79 *
80 * @fn void RobotinoComThread::set_digital_output(unsigned int digital_out, bool enable) = 0
81 * Set digital output state.
82 * @param digital_out digital output as written on the robot, i.e., 1 to 8
83 * @param enable true to enable output, false to disable
84 */
85
86/** @class RobotinoComThread::SensorData "com_thread.h"
87 * Struct to exchange data between com and sensor thread.
88 */
89
90/** Constructor. */
92: seq(0),
93 mot_velocity{0, 0, 0},
94 mot_position{0, 0, 0},
95 mot_current{0., 0., 0.},
96 bumper(false),
97 bumper_estop_enabled(false),
98 digital_in{0, 0, 0, 0, 0, 0, 0, 0},
99 digital_out{0, 0, 0, 0, 0, 0, 0, 0},
100 analog_in{0., 0., 0., 0., 0., 0., 0., 0.},
101 bat_voltage(0.),
102 bat_current(0.),
103 imu_enabled(false),
104 imu_orientation{0., 0., 0., 0.},
105 imu_angular_velocity{0., 0., 0.},
106 imu_angular_velocity_covariance{0., 0., 0., 0., 0., 0., 0., 0., 0.},
107 ir_voltages{0., 0., 0., 0., 0., 0., 0., 0., 0.}
108{
109}
110
111/** Constructor.
112 * @param thread_name name of thread
113 */
115: Thread(thread_name, Thread::OPMODE_CONTINUOUS)
116{
117 data_mutex_ = new Mutex();
118 new_data_ = false;
119
120 vel_mutex_ = new Mutex();
121 vel_last_update_ = new Time();
122 vel_last_zero_ = false;
123 des_vx_ = 0.;
124 des_vy_ = 0.;
125 des_omega_ = 0.;
126
127 set_vx_ = 0.;
128 set_vy_ = 0.;
129 set_omega_ = 0.;
130
131 cfg_rb_ = 0.;
132 cfg_rw_ = 0.;
133 cfg_gear_ = 0.;
134
135 cfg_trans_accel_ = 0.;
136 cfg_trans_decel_ = 0.;
137 cfg_rot_accel_ = 0.;
138 cfg_rot_decel_ = 0.;
139
140#ifdef USE_VELOCITY_RECORDING
141 f_ = fopen("comdata.csv", "w");
142 start_ = new Time();
143#endif
144}
145
146/** Destructor. */
148{
149 delete data_mutex_;
150 delete vel_mutex_;
151 delete vel_last_update_;
152#ifdef USE_VELOCITY_RECORDING
153 fclose(f_);
154 delete start_;
155#endif
156}
157
158/** Get all current sensor data.
159 * @param sensor_data upon return (true) contains the latest available
160 * sensor data
161 * @return true if new data was available and has been stored in \p
162 * sensor_data, false otherwise
163 */
164bool
166{
168 if (new_data_) {
169 sensor_data = data_;
170 new_data_ = false;
171 return true;
172 } else {
173 return false;
174 }
175}
176
177/** Set omni drive layout parameters.
178 * @param rb Distance from Robotino center to wheel center in meters
179 * @param rw Wheel radius in meters
180 * @param gear Gear ratio between motors and wheels
181 */
182void
183RobotinoComThread::set_drive_layout(float rb, float rw, float gear)
184{
185 cfg_rb_ = rb;
186 cfg_rw_ = rw;
187 cfg_gear_ = gear;
188}
189
190/** Set the omni drive limits.
191 * @param trans_accel maximum acceleration in translation
192 * @param trans_decel maximum deceleration in translation
193 * @param rot_accel maximum acceleration in rotation
194 * @param rot_decel maximum deceleration in rotation
195 */
196void
198 float trans_decel,
199 float rot_accel,
200 float rot_decel)
201{
202 cfg_trans_accel_ = trans_accel;
203 cfg_trans_decel_ = trans_decel;
204 cfg_rot_accel_ = rot_accel;
205 cfg_rot_decel_ = rot_decel;
206}
207
208/** Set desired velocities.
209 * @param vx desired velocity in base_link frame X direction ("forward")
210 * @param vy desired velocity in base_link frame Y direction ("sideward")
211 * @param omega desired rotational velocity
212 */
213void
214RobotinoComThread::set_desired_vel(float vx, float vy, float omega)
215{
216 des_vx_ = vx;
217 des_vy_ = vy;
218 des_omega_ = omega;
219}
220
221/** Update velocity values.
222 * This method must be called periodically while driving to update the controller.
223 * @return true if the method must be called again, false otherwise
224 */
225bool
227{
228 bool set_speed = false;
229
230 Time now(clock);
231 float diff_sec = now - vel_last_update_;
232 *vel_last_update_ = now;
233
234 set_vx_ = update_speed(des_vx_, set_vx_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
235 set_vy_ = update_speed(des_vy_, set_vy_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
236 set_omega_ = update_speed(des_omega_, set_omega_, cfg_rot_accel_, cfg_rot_decel_, diff_sec);
237
238 /*
239 logger->log_info(name(), "VX: %.2f -> %.2f (%.2f) VY: %.2f -> %.2f (%.2f) Omg: %.2f -> %.2f (%.2f)",
240 old_set_vx, set_vx_, des_vx_,
241 old_set_vy, set_vy_, des_vy_,
242 old_set_omega, set_omega_);
243 */
244
245 if (set_vx_ == 0.0 && set_vy_ == 0.0 && set_omega_ == 0.0) {
246 if (!vel_last_zero_) {
247 set_speed = true;
248 vel_last_zero_ = true;
249 }
250 } else {
251 set_speed = true;
252 vel_last_zero_ = false;
253 }
254
255 if (set_speed) {
256 float s1 = 0., s2 = 0., s3 = 0.;
257 project(&s1, &s2, &s3, set_vx_, set_vy_, set_omega_);
258 set_speed_points(s1, s2, s3);
259
260#ifdef USE_VELOCITY_RECORDING
261 {
262 Time now(clock);
263 float time_diff = now - start_;
264
265 fprintf(f_,
266 "%f\t%f\t%f\t%f\t%f\t%f\t%f\n",
267 time_diff,
268 des_vx_,
269 set_vx_,
270 des_vy_,
271 set_vy_,
272 des_omega_,
273 set_omega_);
274 }
275#endif
276 }
277
278 return !vel_last_zero_;
279}
280
281float
282RobotinoComThread::update_speed(float des, float set, float accel, float decel, float diff_sec)
283{
284 if (des >= 0 && set < 0) {
285 const float decrement = std::copysign(decel, set) * diff_sec;
286 if (des > set - decrement) {
287 //logger->log_debug(name(), " Case 1a %f %f %f", decrement, decel, diff_sec);
288 set -= decrement;
289 } else {
290 //logger->log_debug(name(), " Case 1b");
291 set = des;
292 }
293
294 } else if (des <= 0 && set > 0) {
295 const float decrement = std::copysign(decel, set) * diff_sec;
296 if (des < set - decrement) {
297 //logger->log_debug(name(), " Case 1c %f %f %f", decrement, decel, diff_sec);
298 set -= decrement;
299 } else {
300 //logger->log_debug(name(), " Case 1d");
301 set = des;
302 }
303
304 } else if (fabs(des) > fabs(set)) {
305 const float increment = std::copysign(accel, des) * diff_sec;
306 if (fabs(des) > fabs(set + increment)) {
307 //logger->log_debug(name(), " Case 2a %f %f", increment, accel, diff_sec);
308 set += increment;
309 } else {
310 //logger->log_debug(name(), " Case 2b");
311 set = des;
312 }
313 } else if (fabs(des) < fabs(set)) {
314 const float decrement = std::copysign(decel, des) * diff_sec;
315 if (fabs(des) < fabs(set - decrement)) {
316 //logger->log_debug(name(), " Case 3a %f %f %f", decrement, decel, diff_sec);
317 set -= decrement;
318 } else {
319 //logger->log_debug(name(), " Case 3b");
320 set = des;
321 }
322 }
323
324 return set;
325}
326
327/** Project the velocity of the robot in cartesian coordinates to single motor speeds.
328 *
329 * From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany.
330 * The code has been released under a 2-clause BSD license.
331 *
332 * @param m1 The resulting speed of motor 1 in rpm
333 * @param m2 The resulting speed of motor 2 in rpm
334 * @param m3 The resulting speed of motor 3 in rpm
335 * @param vx Velocity in x-direction in m/s
336 * @param vy Velocity in y-direction in m/s
337 * @param omega Angular velocity in rad/s
338 */
339//Redistribution and use in source and binary forms, with or without
340//modification, are permitted provided that the following conditions
341//are met:
342//1) Redistributions of source code must retain the above copyright
343//notice, this list of conditions and the following disclaimer.
344//2) Redistributions in binary form must reproduce the above copyright
345//notice, this list of conditions and the following disclaimer in the
346//documentation and/or other materials provided with the distribution.
347//
348//THIS SOFTWARE IS PROVIDED BY REC ROBOTICS EQUIPMENT CORPORATION GMBH
349//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
350//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
351//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL REC
352//ROBOTICS EQUIPMENT CORPORATION GMBH BE LIABLE FOR ANY DIRECT,
353//INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
354//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
355//SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
356//HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
357//STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
358//ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
359//OF THE POSSIBILITY OF SUCH DAMAGE.
360void
361RobotinoComThread::project(float *m1, float *m2, float *m3, float vx, float vy, float omega) const
362{
363 //Projection matrix
364 static const double v0[2] = {-0.5 * sqrt(3.0), 0.5};
365 static const double v1[2] = {0.0, -1.0};
366 static const double v2[2] = {0.5 * sqrt(3.0), 0.5};
367
368 //Scale omega with the radius of the robot
369 double vOmegaScaled = cfg_rb_ * (double)omega;
370
371 //Convert from m/s to RPM
372 const double k = 60.0 * cfg_gear_ / (2.0 * M_PI * cfg_rw_);
373
374 //Compute the desired velocity
375 *m1 = static_cast<float>((v0[0] * (double)vx + v0[1] * (double)vy + vOmegaScaled) * k);
376 *m2 = static_cast<float>((v1[0] * (double)vx + v1[1] * (double)vy + vOmegaScaled) * k);
377 *m3 = static_cast<float>((v2[0] * (double)vx + v2[1] * (double)vy + vOmegaScaled) * k);
378}
379
380/** Project single motor speeds to velocity in cartesian coordinates.
381 *
382 * From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany.
383 * The code has been released under a 2-clause BSD license.
384 *
385 * @param vx The resulting speed in x-direction in m/s
386 * @param vy The resulting speed in y-direction in m/s
387 * @param omega The resulting angular velocity in rad/s
388 * @param m1 Speed of motor 1 in rpm
389 * @param m2 Speed of motor 2 in rpm
390 * @param m3 Speed of motor 3 in rpm
391 * @throws RobotinoException if no valid drive layout parameters are available.
392 */
393void
394RobotinoComThread::unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const
395{
396 //Convert from RPM to mm/s
397 const double k = 60.0 * cfg_gear_ / (2.0 * M_PI * cfg_rw_);
398
399 *vx = static_cast<float>(((double)m3 - (double)m1) / sqrt(3.0) / k);
400 *vy =
401 static_cast<float>(2.0 / 3.0 * ((double)m1 + 0.5 * ((double)m3 - (double)m1) - (double)m2) / k);
402
403 double vw = (double)*vy + (double)m2 / k;
404
405 *omega = static_cast<float>(vw / cfg_rb_);
406}
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 set_speed_points(float s1, float s2, float s3)=0
Set speed points for wheels.
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
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
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
Thread class encapsulation of pthreads.
Definition: thread.h:46
@ OPMODE_CONTINUOUS
operate in continuous mode (default)
Definition: thread.h:57
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