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 
27 using 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  */
114 RobotinoComThread::RobotinoComThread(const char *thread_name)
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  */
164 bool
166 {
167  MutexLocker lock(data_mutex_);
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  */
182 void
183 RobotinoComThread::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  */
196 void
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  */
213 void
214 RobotinoComThread::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  */
225 bool
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 
281 float
282 RobotinoComThread::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.
360 void
361 RobotinoComThread::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  */
393 void
394 RobotinoComThread::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