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), mot_velocity{0,0,0}, mot_position{0,0,0}, mot_current{0.,0.,0.},
93  bumper(false), bumper_estop_enabled(false), digital_in{0,0,0,0,0,0,0,0},
94  digital_out{0,0,0,0,0,0,0,0},
95  analog_in{0.,0.,0.,0.,0.,0.,0.,0.}, bat_voltage(0.), bat_current(0.),
96  imu_enabled(false), imu_orientation{0.,0.,0.,0.}, imu_angular_velocity{0.,0.,0.},
97  imu_angular_velocity_covariance{0.,0.,0.,0.,0.,0.,0.,0.,0.},
98  ir_voltages{0.,0.,0.,0.,0.,0.,0.,0.,0.}
99 {
100 }
101 
102 /** Constructor.
103  * @param thread_name name of thread
104  */
105 RobotinoComThread::RobotinoComThread(const char *thread_name)
106  : Thread(thread_name, Thread::OPMODE_CONTINUOUS)
107 {
108  data_mutex_ = new Mutex();
109  new_data_ = false;
110 
111  vel_mutex_ = new Mutex();
112  vel_last_update_ = new Time();
113  vel_last_zero_ = false;
114  des_vx_ = 0.;
115  des_vy_ = 0.;
116  des_omega_ = 0.;
117 
118  set_vx_ = 0.;
119  set_vy_ = 0.;
120  set_omega_ = 0.;
121 
122  cfg_rb_ = 0.;
123  cfg_rw_ = 0.;
124  cfg_gear_ = 0.;
125 
126  cfg_trans_accel_ = 0.;
127  cfg_trans_decel_ = 0.;
128  cfg_rot_accel_ = 0.;
129  cfg_rot_decel_ = 0.;
130 
131 #ifdef USE_VELOCITY_RECORDING
132  f_ = fopen("comdata.csv", "w");
133  start_ = new Time();
134 #endif
135 }
136 
137 
138 /** Destructor. */
140 {
141  delete data_mutex_;
142 #ifdef USE_VELOCITY_RECORDING
143  fclose(f_);
144 #endif
145 }
146 
147 
148 /** Get all current sensor data.
149  * @param sensor_data upon return (true) contains the latest available
150  * sensor data
151  * @return true if new data was available and has been stored in \p
152  * sensor_data, false otherwise
153  */
154 bool
156 {
157  MutexLocker lock(data_mutex_);
158  if (new_data_) {
159  sensor_data = data_;
160  new_data_ = false;
161  return true;
162  } else {
163  return false;
164  }
165 }
166 
167 
168 /** Set omni drive layout parameters.
169  * @param rb Distance from Robotino center to wheel center in meters
170  * @param rw Wheel radius in meters
171  * @param gear Gear ratio between motors and wheels
172  */
173 void
174 RobotinoComThread::set_drive_layout(float rb, float rw, float gear)
175 {
176  cfg_rb_ = rb;
177  cfg_rw_ = rw;
178  cfg_gear_ = gear;
179 }
180 
181 
182 /** Set the omni drive limits.
183  * @param trans_accel maximum acceleration in translation
184  * @param trans_decel maximum deceleration in translation
185  * @param rot_accel maximum acceleration in rotation
186  * @param rot_decel maximum deceleration in rotation
187  */
188 void
189 RobotinoComThread::set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel)
190 {
191  cfg_trans_accel_ = trans_accel;
192  cfg_trans_decel_ = trans_decel;
193  cfg_rot_accel_ = rot_accel;
194  cfg_rot_decel_ = rot_decel;
195 }
196 
197 
198 /** Set desired velocities.
199  * @param vx desired velocity in base_link frame X direction ("forward")
200  * @param vy desired velocity in base_link frame Y direction ("sideward")
201  * @param omega desired rotational velocity
202  */
203 void
204 RobotinoComThread::set_desired_vel(float vx, float vy, float omega)
205 {
206  des_vx_ = vx;
207  des_vy_ = vy;
208  des_omega_ = omega;
209 }
210 
211 
212 /** Update velocity values.
213  * This method must be called periodically while driving to update the controller.
214  * @return true if the method must be called again, false otherwise
215  */
216 bool
218 {
219  bool set_speed = false;
220 
221  Time now(clock);
222  float diff_sec = now - vel_last_update_;
223  *vel_last_update_ = now;
224 
225  set_vx_ = update_speed(des_vx_, set_vx_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
226  set_vy_ = update_speed(des_vy_, set_vy_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
227  set_omega_ = update_speed(des_omega_, set_omega_, cfg_rot_accel_, cfg_rot_decel_, diff_sec);
228 
229  /*
230  logger->log_info(name(), "VX: %.2f -> %.2f (%.2f) VY: %.2f -> %.2f (%.2f) Omg: %.2f -> %.2f (%.2f)",
231  old_set_vx, set_vx_, des_vx_,
232  old_set_vy, set_vy_, des_vy_,
233  old_set_omega, set_omega_);
234  */
235 
236  if (set_vx_ == 0.0 && set_vy_ == 0.0 && set_omega_ == 0.0) {
237  if (! vel_last_zero_) {
238  set_speed = true;
239  vel_last_zero_ = true;
240  }
241  } else {
242  set_speed = true;
243  vel_last_zero_ = false;
244  }
245 
246  if (set_speed) {
247  float s1 = 0., s2 = 0., s3 = 0.;
248  project(&s1, &s2, &s3, set_vx_, set_vy_, set_omega_);
249  set_speed_points(s1, s2, s3);
250 
251 #ifdef USE_VELOCITY_RECORDING
252  {
253  Time now(clock);
254  float time_diff = now - start_;
255 
256  fprintf(f_, "%f\t%f\t%f\t%f\t%f\t%f\t%f\n", time_diff,
257  des_vx_, set_vx_, des_vy_, set_vy_, des_omega_, set_omega_);
258  }
259 #endif
260 
261  }
262 
263  return ! vel_last_zero_;
264 }
265 
266 float
267 RobotinoComThread::update_speed(float des, float set, float accel, float decel, float diff_sec)
268 {
269  if (des >= 0 && set < 0) {
270  const float decrement = std::copysign(decel, set) * diff_sec;
271  if (des > set - decrement) {
272  //logger->log_debug(name(), " Case 1a %f %f %f", decrement, decel, diff_sec);
273  set -= decrement;
274  } else {
275  //logger->log_debug(name(), " Case 1b");
276  set = des;
277  }
278 
279  } else if (des <= 0 && set > 0) {
280  const float decrement = std::copysign(decel, set) * diff_sec;
281  if (des < set - decrement ) {
282  //logger->log_debug(name(), " Case 1c %f %f %f", decrement, decel, diff_sec);
283  set -= decrement;
284  } else {
285  //logger->log_debug(name(), " Case 1d");
286  set = des;
287  }
288 
289  } else if (fabs(des) > fabs(set)) {
290  const float increment = std::copysign(accel, des) * diff_sec;
291  if (fabs(des) > fabs(set + increment)) {
292  //logger->log_debug(name(), " Case 2a %f %f", increment, accel, diff_sec);
293  set += increment;
294  } else {
295  //logger->log_debug(name(), " Case 2b");
296  set = des;
297  }
298  } else if (fabs(des) < fabs(set)) {
299  const float decrement = std::copysign(decel, des) * diff_sec;
300  if (fabs(des) < fabs(set - decrement)) {
301  //logger->log_debug(name(), " Case 3a %f %f %f", decrement, decel, diff_sec);
302  set -= decrement;
303  } else {
304  //logger->log_debug(name(), " Case 3b");
305  set = des;
306  }
307  }
308 
309  return set;
310 }
311 
312 /** Project the velocity of the robot in cartesian coordinates to single motor speeds.
313  *
314  * From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany.
315  * The code has been released under a 2-clause BSD license.
316  *
317  * @param m1 The resulting speed of motor 1 in rpm
318  * @param m2 The resulting speed of motor 2 in rpm
319  * @param m3 The resulting speed of motor 3 in rpm
320  * @param vx Velocity in x-direction in m/s
321  * @param vy Velocity in y-direction in m/s
322  * @param omega Angular velocity in rad/s
323  */
324 //Redistribution and use in source and binary forms, with or without
325 //modification, are permitted provided that the following conditions
326 //are met:
327 //1) Redistributions of source code must retain the above copyright
328 //notice, this list of conditions and the following disclaimer.
329 //2) Redistributions in binary form must reproduce the above copyright
330 //notice, this list of conditions and the following disclaimer in the
331 //documentation and/or other materials provided with the distribution.
332 //
333 //THIS SOFTWARE IS PROVIDED BY REC ROBOTICS EQUIPMENT CORPORATION GMBH
334 //"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
335 //LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
336 //FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL REC
337 //ROBOTICS EQUIPMENT CORPORATION GMBH BE LIABLE FOR ANY DIRECT,
338 //INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
339 //(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
340 //SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
341 //HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
342 //STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
343 //ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
344 //OF THE POSSIBILITY OF SUCH DAMAGE.
345 void
346 RobotinoComThread::project( float* m1, float* m2, float* m3, float vx, float vy, float omega ) const
347 {
348  //Projection matrix
349  static const double v0[2] = { -0.5 * sqrt( 3.0 ), 0.5 };
350  static const double v1[2] = { 0.0 , -1.0 };
351  static const double v2[2] = { 0.5 * sqrt( 3.0 ), 0.5 };
352 
353  //Scale omega with the radius of the robot
354  double vOmegaScaled = cfg_rb_ * (double)omega ;
355 
356  //Convert from m/s to RPM
357  const double k = 60.0 * cfg_gear_ / ( 2.0 * M_PI * cfg_rw_ );
358 
359  //Compute the desired velocity
360  *m1 = static_cast<float>( ( v0[0] * (double)vx + v0[1] * (double)vy + vOmegaScaled ) * k );
361  *m2 = static_cast<float>( ( v1[0] * (double)vx + v1[1] * (double)vy + vOmegaScaled ) * k );
362  *m3 = static_cast<float>( ( v2[0] * (double)vx + v2[1] * (double)vy + vOmegaScaled ) * k );
363 }
364 
365 /** Project single motor speeds to velocity in cartesian coordinates.
366  *
367  * From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany.
368  * The code has been released under a 2-clause BSD license.
369  *
370  * @param vx The resulting speed in x-direction in m/s
371  * @param vy The resulting speed in y-direction in m/s
372  * @param omega The resulting angular velocity in rad/s
373  * @param m1 Speed of motor 1 in rpm
374  * @param m2 Speed of motor 2 in rpm
375  * @param m3 Speed of motor 3 in rpm
376  * @throws RobotinoException if no valid drive layout parameters are available.
377  */
378 void
379 RobotinoComThread::unproject( float* vx, float* vy, float* omega, float m1, float m2, float m3 ) const
380 {
381  //Convert from RPM to mm/s
382  const double k = 60.0 * cfg_gear_ / ( 2.0 * M_PI * cfg_rw_ );
383 
384  *vx = static_cast<float>( ( (double)m3 - (double)m1 ) / sqrt( 3.0 ) / k );
385  *vy = static_cast<float>( 2.0 / 3.0 * ( (double)m1 + 0.5 * ( (double)m3 - (double)m1 ) - (double)m2 ) / k );
386 
387  double vw = (double)*vy + (double)m2 / k;
388 
389  *omega = static_cast<float>( vw / cfg_rb_ );
390 }
SensorData data_
Data struct that must be updated whenever new data is available.
Definition: com_thread.h:116
virtual ~RobotinoComThread()
Destructor.
Definition: com_thread.cpp:139
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:379
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:45
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
Definition: com_thread.h:118
Fawkes library namespace.
Mutex locking helper.
Definition: mutex_locker.h:33
bool update_velocities()
Update velocity values.
Definition: com_thread.cpp:217
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 void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
Definition: com_thread.cpp:204
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
void set_drive_layout(float rb, float rw, float gear)
Set omni drive layout parameters.
Definition: com_thread.cpp:174
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:346
operate in continuous mode (default)
Definition: thread.h:53
virtual bool get_data(SensorData &sensor_data)
Get all current sensor data.
Definition: com_thread.cpp:155
RobotinoComThread(const char *thread_name)
Constructor.
Definition: com_thread.cpp:105
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:189
Mutex mutual exclusion lock.
Definition: mutex.h:32