Fawkes API  Fawkes Development Version
openrobotino_com_thread.cpp
1 
2 /***************************************************************************
3  * openrobotino_com_thread.cpp - Robotino com thread
4  *
5  * Created: Thu Sep 11 13:18:00 2014
6  * Copyright 2011-2014 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 "openrobotino_com_thread.h"
23 #ifdef HAVE_OPENROBOTINO_API_1
24 # include <rec/iocontrol/remotestate/SensorState.h>
25 # include <rec/iocontrol/robotstate/State.h>
26 # include <rec/robotino/com/Com.h>
27 # include <rec/sharedmemory/sharedmemory.h>
28 #else
29 # include <rec/robotino/api2/AnalogInputArray.h>
30 # include <rec/robotino/api2/Bumper.h>
31 # include <rec/robotino/api2/Com.h>
32 # include <rec/robotino/api2/DigitalInputArray.h>
33 # include <rec/robotino/api2/DistanceSensorArray.h>
34 # include <rec/robotino/api2/ElectricalGripper.h>
35 # include <rec/robotino/api2/Gyroscope.h>
36 # include <rec/robotino/api2/MotorArray.h>
37 # include <rec/robotino/api2/Odometry.h>
38 # include <rec/robotino/api2/PowerManagement.h>
39 #endif
40 #include <baseapp/run.h>
41 #include <core/threading/mutex.h>
42 #include <core/threading/mutex_locker.h>
43 #include <tf/types.h>
44 #include <utils/math/angle.h>
45 #include <utils/time/wait.h>
46 
47 #include <unistd.h>
48 
49 using namespace fawkes;
50 
51 /** @class OpenRobotinoComThread "openrobotino_com_thread.h"
52  * Thread to communicate with Robotino via OpenRobotino API (v1 or v2).
53  * @author Tim Niemueller
54  */
55 
56 /** Constructor. */
58 {
59 #ifdef HAVE_OPENROBOTINO_API_1
60  com_ = this;
61 #endif
62 }
63 
64 /** Destructor. */
66 {
67 }
68 
69 void
71 {
72  cfg_hostname_ = config->get_string("/hardware/robotino/openrobotino/hostname");
73  cfg_quit_on_disconnect_ = config->get_bool("/hardware/robotino/openrobotino/quit_on_disconnect");
74  cfg_sensor_update_cycle_time_ = config->get_uint("/hardware/robotino/cycle-time");
75  cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
76  cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
77 
78 #ifdef HAVE_OPENROBOTINO_API_1
79  statemem_ = new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>(
80  rec::iocontrol::robotstate::State::sharedMemoryKey);
81  state_ = statemem_->getData();
82  state_mutex_ = new Mutex();
83  set_state_ = new rec::iocontrol::remotestate::SetState();
84  set_state_->gripper_isEnabled = cfg_gripper_enabled_;
85  active_state_ = 0;
86 #endif
87 
88  last_seqnum_ = 0;
89  time_wait_ = new TimeWait(clock, cfg_sensor_update_cycle_time_ * 1000);
90 
91  if (cfg_enable_gyro_) {
92  data_.imu_enabled = true;
93 
94  for (int i = 0; i < 3; ++i)
95  data_.imu_angular_velocity[i] = 0.;
96  for (int i = 0; i < 4; ++i)
97  data_.imu_orientation[i] = 0.;
98  for (int i = 0; i < 9; ++i)
99  data_.imu_angular_velocity_covariance[i] = 0.;
100  // Assume that the gyro is the CruizCore XG1010 and thus set data
101  // from datasheet
102  data_.imu_angular_velocity_covariance[8] = deg2rad(0.1);
103  }
104 
105 #ifdef HAVE_OPENROBOTINO_API_1
106  com_->setAddress(cfg_hostname_.c_str());
107  com_->setMinimumUpdateCycleTime(cfg_sensor_update_cycle_time_);
108  com_->connect(/* blocking */ false);
109 #else
110  com_ = new rec::robotino::api2::Com("Fawkes");
111  com_->setAddress(cfg_hostname_.c_str());
112  com_->setAutoReconnectEnabled(false);
113  com_->connectToServer(/* blocking */ true);
114 
115  analog_inputs_com_ = new rec::robotino::api2::AnalogInputArray();
116  bumper_com_ = new rec::robotino::api2::Bumper();
117  digital_inputs_com_ = new rec::robotino::api2::DigitalInputArray();
118  distances_com_ = new rec::robotino::api2::DistanceSensorArray();
119  gripper_com_ = new rec::robotino::api2::ElectricalGripper();
120  gyroscope_com_ = new rec::robotino::api2::Gyroscope();
121  motors_com_ = new rec::robotino::api2::MotorArray();
122  odom_com_ = new rec::robotino::api2::Odometry();
123  power_com_ = new rec::robotino::api2::PowerManagement();
124 
125  analog_inputs_com_->setComId(com_->id());
126  bumper_com_->setComId(com_->id());
127  digital_inputs_com_->setComId(com_->id());
128  distances_com_->setComId(com_->id());
129  gripper_com_->setComId(com_->id());
130  gyroscope_com_->setComId(com_->id());
131  motors_com_->setComId(com_->id());
132  odom_com_->setComId(com_->id());
133  power_com_->setComId(com_->id());
134 #endif
135 }
136 
137 void
139 {
140  delete time_wait_;
141 #ifdef HAVE_OPENROBOTINO_API_1
142  set_state_->speedSetPoint[0] = 0.;
143  set_state_->speedSetPoint[1] = 0.;
144  set_state_->speedSetPoint[2] = 0.;
145  set_state_->gripper_isEnabled = false;
146  com_->setSetState(*set_state_);
147  usleep(50000);
148  delete set_state_;
149  delete state_mutex_;
150  delete statemem_;
151 #else
152  float speeds[3] = {0, 0, 0};
153  motors_com_->setSpeedSetPoints(speeds, 3);
154  usleep(50000);
155  delete analog_inputs_com_;
156  delete bumper_com_;
157  delete digital_inputs_com_;
158  delete distances_com_;
159  delete gripper_com_;
160  delete gyroscope_com_;
161  delete motors_com_;
162  delete odom_com_;
163  delete power_com_;
164  delete com_;
165 #endif
166 }
167 
168 void
170 {
171  reset_odometry();
172 }
173 
174 void
176 {
177  time_wait_->mark_start();
178 
179  if (com_->isConnected()) {
180  MutexLocker lock(data_mutex_);
181 #ifdef HAVE_OPENROBOTINO_API_1
182  process_api_v1();
183 #else
184  process_api_v2();
185 #endif
186 
187 #ifdef HAVE_OPENROBOTINO_API_1
188  } else if (com_->connectionState() == rec::robotino::com::Com::NotConnected) {
189 #else
190  } else {
191 #endif
192  if (cfg_quit_on_disconnect_) {
193  logger->log_warn(name(), "Connection lost, quitting (as per config)");
194  fawkes::runtime::quit();
195  } else {
196  // retry connection
197 #ifdef HAVE_OPENROBOTINO_API_1
198  com_->connect(/* blocking */ false);
199 #else
200  com_->connectToServer(/* blocking */ true);
201 #endif
202  }
203  }
204 
205  time_wait_->wait();
206 }
207 
208 void
209 OpenRobotinoComThread::process_api_v1()
210 {
211 #ifdef HAVE_OPENROBOTINO_API_1
212  state_mutex_->lock();
213  fawkes::Time sensor_time = times_[active_state_];
214  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
215  state_mutex_->unlock();
216 
217  if (sensor_state.sequenceNumber != last_seqnum_) {
218  new_data_ = true;
219  last_seqnum_ = sensor_state.sequenceNumber;
220 
221  // update sensor values in interface
222  for (int i = 0; i < 3; ++i) {
223  data_.mot_velocity[i] = sensor_state.actualVelocity[i];
224  data_.mot_position[i] = sensor_state.actualPosition[i];
225  data_.mot_current[i] = sensor_state.motorCurrent[i];
226  }
227  data_.bumper = sensor_state.bumper;
228  data_.bumper_estop_enabled = state_->emergencyStop.isEnabled;
229  for (int i = 0; i < 8; ++i) {
230  data_.digital_in[i] = sensor_state.dIn[i];
231  data_.analog_in[i] = sensor_state.aIn[i];
232  }
233  if (cfg_enable_gyro_) {
234  if (state_->gyro.port == rec::serialport::UNDEFINED) {
235  if (fabs(data_.imu_angular_velocity[0] + 1.) > 0.00001) {
236  data_.imu_angular_velocity[0] = -1.;
237  data_.imu_angular_velocity[2] = 0.;
238  data_.imu_orientation[0] = -1.;
239  }
240  } else {
241  data_.imu_angular_velocity[0] = 0.;
242  data_.imu_angular_velocity[2] = state_->gyro.rate;
243 
244  tf::Quaternion q = tf::create_quaternion_from_yaw(state_->gyro.angle);
245  data_.imu_orientation[0] = q.x();
246  data_.imu_orientation[1] = q.y();
247  data_.imu_orientation[2] = q.z();
248  data_.imu_orientation[3] = q.w();
249  }
250  }
251 
252  for (int i = 0; i < NUM_IR_SENSORS; ++i) {
253  data_.ir_voltages[i] = sensor_state.distanceSensor[i];
254  }
255 
256  data_.bat_voltage = roundf(sensor_state.voltage * 1000.);
257  data_.bat_current = roundf(sensor_state.current);
258 
259  // 21.0V is empty, 26.0V is empty, from OpenRobotino lcdd
260  float soc = (sensor_state.voltage - 21.0f) / 5.f;
261  soc = std::min(1.f, std::max(0.f, soc));
262  data_.bat_absolute_soc = soc;
263  }
264 #endif
265 }
266 
267 void
268 OpenRobotinoComThread::process_api_v2()
269 {
270 #ifdef HAVE_OPENROBOTINO_API_2
271  com_->processComEvents();
272 
273  double odo_x = 0, odo_y = 0, odo_phi = 0;
274  unsigned int odo_seq = 0;
275 
276  odom_com_->readings(&odo_x, &odo_y, &odo_phi, &odo_seq);
277 
278  if (odo_seq != last_seqnum_) {
279  new_data_ = true;
280  last_seqnum_ = odo_seq;
281 
282  if (motors_com_->numMotors() != 3) {
283  logger->log_error(name(),
284  "Invalid number of motors, got %u, expected 3",
285  motors_com_->numMotors());
286  return;
287  }
288  motors_com_->actualVelocities(data_.mot_velocity);
289  motors_com_->actualPositions(data_.mot_position);
290  motors_com_->motorCurrents(data_.mot_current);
291 
292  data_.bumper = bumper_com_->value();
293 
294  if (digital_inputs_com_->numDigitalInputs() != 8) {
295  logger->log_error(name(),
296  "Invalid number of digital inputs, got %u, expected 8",
297  digital_inputs_com_->numDigitalInputs());
298  return;
299  }
300  int digin_readings[8];
301  digital_inputs_com_->values(digin_readings);
302  for (unsigned int i = 0; i < 8; ++i)
303  data_.digital_in[i] = (digin_readings[i] != 0);
304 
305  if (analog_inputs_com_->numAnalogInputs() != 8) {
306  logger->log_error(name(),
307  "Invalid number of analog inputs, got %u, expected 8",
308  analog_inputs_com_->numAnalogInputs());
309  return;
310  }
311  analog_inputs_com_->values(data_.analog_in);
312 
313  if (distances_com_->numDistanceSensors() != NUM_IR_SENSORS) {
314  logger->log_error(name(),
315  "Invalid number of distance sensors, got %u, expected 9",
316  distances_com_->numDistanceSensors());
317  return;
318  }
319  // the distance calculation from API2 uses a max value of 0.41,
320  // which breaks the previous behavior of 0.0 for "nothing"
321  // therefore use our API1 conversion routine
322  distances_com_->voltages(data_.ir_voltages);
323 
324  float pow_current = power_com_->current() * 1000.; // A -> mA
325  float pow_voltage = power_com_->voltage() * 1000.; // V -> mV
326 
327  float gyro_angle = gyroscope_com_->angle();
328  float gyro_rate = gyroscope_com_->rate();
329 
330  data_.bumper_estop_enabled = false;
331  data_.imu_angular_velocity[0] = 0.;
332  data_.imu_angular_velocity[2] = gyro_rate;
333 
334  tf::Quaternion q = tf::create_quaternion_from_yaw(gyro_angle);
335  data_.imu_orientation[0] = q.x();
336  data_.imu_orientation[1] = q.y();
337  data_.imu_orientation[2] = q.z();
338  data_.imu_orientation[3] = q.w();
339 
340  data_.bat_voltage = roundf(pow_voltage);
341  ;
342  data_.bat_current = roundf(pow_current);
343 
344  // 22.0V is empty, 24.5V is full, this is just a guess
345  float soc = (power_com_->voltage() - 22.0f) / 2.5f;
346  soc = std::min(1.f, std::max(0.f, soc));
347  data_.bat_absolute_soc = soc;
348  }
349 #endif
350 }
351 
352 #ifdef HAVE_OPENROBOTINO_API_1
353 /** Update event. */
354 void
355 OpenRobotinoComThread::updateEvent()
356 {
357  unsigned int next_state = 1 - active_state_;
358  sensor_states_[next_state] = sensorState();
359  times_[next_state].stamp();
360 
361  MutexLocker lock(state_mutex_);
362  active_state_ = next_state;
363 }
364 #endif
365 
366 /** Reset odometry to zero. */
367 void
369 {
370  if (com_->isConnected()) {
371 #ifdef HAVE_OPENROBOTINO_API_1
372  set_state_->setOdometry = true;
373  set_state_->odometryX = set_state_->odometryY = set_state_->odometryPhi = 0.;
374  com_->setSetState(*set_state_);
375  set_state_->setOdometry = false;
376 #else
377  odom_com_->set(0., 0., 0., /* blocking */ true);
378 #endif
379  }
380 }
381 
382 void
383 OpenRobotinoComThread::set_motor_accel_limits(float min_accel, float max_accel)
384 {
385  throw Exception(
386  "Setting motor accel limits for OpenRobotino driver not supported, configure controld3");
387 }
388 
389 void
390 OpenRobotinoComThread::set_digital_output(unsigned int digital_out, bool enable)
391 {
392  logger->log_error(name(), "Setting digital outputs not supported with openrobotino driver");
393 }
394 
395 /** Check if we are connected to OpenRobotino.
396  * @return true if the connection has been established, false otherwise
397  */
398 bool
400 {
401  return com_->isConnected();
402 }
403 
404 /** Get actual velocity.
405  * @param a1 upon return contains velocity in RPM for first wheel
406  * @param a2 upon return contains velocity in RPM for second wheel
407  * @param a3 upon return contains velocity in RPM for third wheel
408  * @param seq upon return contains sequence number of latest data
409  * @param t upon return contains time of latest data
410  */
411 void
413  float & a2,
414  float & a3,
415  unsigned int &seq,
416  fawkes::Time &t)
417 {
418  MutexLocker lock(data_mutex_);
419 
420 #ifdef HAVE_OPENROBOTINO_API_1
421  state_mutex_->lock();
422  t = times_[active_state_];
423  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
424  state_mutex_->unlock();
425 
426  // div by 1000 to convert from mm to m
427  a1 = sensor_state.actualVelocity[0] / 1000.f;
428  a2 = sensor_state.actualVelocity[1] / 1000.f;
429  a3 = sensor_state.actualVelocity[2] / 1000.f;
430  seq = sensor_state.sequenceNumber;
431 #else
432  t.stamp();
433 
434  float mot_act_vel[motors_com_->numMotors()];
435  motors_com_->actualVelocities(mot_act_vel);
436 
437  double odo_x = 0, odo_y = 0, odo_phi = 0;
438  odom_com_->readings(&odo_x, &odo_y, &odo_phi, &seq);
439 
440  a1 = mot_act_vel[0];
441  a2 = mot_act_vel[1];
442  a3 = mot_act_vel[2];
443 #endif
444 }
445 
446 /** Get current odometry.
447  * @param x X coordinate of robot in odometry frame
448  * @param y Y coordinate of robot in odometry frame
449  * @param phi orientation of robot in odometry frame
450  */
451 void
452 OpenRobotinoComThread::get_odometry(double &x, double &y, double &phi)
453 {
454  MutexLocker lock(data_mutex_);
455 
456 #ifdef HAVE_OPENROBOTINO_API_1
457  state_mutex_->lock();
458  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
459  state_mutex_->unlock();
460 
461  x = sensor_state.odometryX / 1000.f;
462  y = sensor_state.odometryY / 1000.f;
463  phi = deg2rad(sensor_state.odometryPhi);
464 
465 #else
466  unsigned int seq;
467  odom_com_->readings(&x, &y, &phi, &seq);
468 #endif
469 }
470 
471 /** Check if gripper is open.
472  * @return true if the gripper is presumably open, false otherwise
473  */
474 bool
476 {
477  MutexLocker lock(data_mutex_);
478 
479 #ifdef HAVE_OPENROBOTINO_API_1
480  state_mutex_->lock();
481  rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
482  state_mutex_->unlock();
483 
484  return sensor_state.isGripperOpened;
485 #else
486  return gripper_com_->isOpened();
487 #endif
488 }
489 
490 /** Set speed points for wheels.
491  * @param s1 speed point for first wheel in RPM
492  * @param s2 speed point for second wheel in RPM
493  * @param s3 speed point for third wheel in RPM
494  */
495 void
496 OpenRobotinoComThread::set_speed_points(float s1, float s2, float s3)
497 {
498 #ifdef HAVE_OPENROBOTINO_API_1
499  set_state_->speedSetPoint[0] = s1;
500  set_state_->speedSetPoint[1] = s2;
501  set_state_->speedSetPoint[2] = s3;
502 
503  com_->setSetState(*set_state_);
504 #else
505  float speeds[3] = {s1, s2, s3};
506  motors_com_->setSpeedSetPoints(speeds, 3);
507 #endif
508 }
509 
510 /** Open or close gripper.
511  * @param opened true to open gripper, false to close
512  */
513 void
515 {
516 #ifdef HAVE_OPENROBOTINO_API_1
517  set_state_->gripper_close = !opened;
518  com_->setSetState(*set_state_);
519 #else
520  if (opened) {
521  gripper_com_->open();
522  } else {
523  gripper_com_->close();
524  }
525 #endif
526 }
527 
528 void
530 {
531 #ifdef HAVE_OPENROBOTINO_API_1
532  logger->log_info(name(), "%sabling bumper estop on request", msg->is_enabled() ? "En" : "Dis");
533  state_->emergencyStop.isEnabled = msg->is_enabled();
534 #else
535  logger->log_info(name(), "Setting bumper estop not supported for API2");
536 #endif
537 }
virtual void set_digital_output(unsigned int digital_out, bool enable)
Set digital output state.
virtual void set_gripper(bool opened)
Open or close gripper.
virtual bool is_connected()
Check if we are connected to OpenRobotino.
virtual void init()
Initialize the thread.
virtual void once()
Execute an action exactly once.
virtual void loop()
Code to execute in the thread.
virtual void set_bumper_estop_enabled(bool enabled)
Enable or disable emergency stop on bumper contact.
virtual void get_odometry(double &x, double &y, double &phi)
Get current odometry.
virtual void reset_odometry()
Reset odometry to zero.
virtual void set_speed_points(float s1, float s2, float s3)
Set speed points for wheels.
virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)
Get actual velocity.
virtual void set_motor_accel_limits(float min_accel, float max_accel)
Set acceleration limits of motors.
virtual bool is_gripper_open()
Check if gripper is open.
virtual ~OpenRobotinoComThread()
Destructor.
virtual void finalize()
Finalize the thread.
Virtual base class for thread that communicates with a Robotino.
Definition: com_thread.h:41
fawkes::Mutex * data_mutex_
Mutex to protect data_.
Definition: com_thread.h:112
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
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
const char * name() const
Get name of thread.
Definition: thread.h:100
Time wait utility.
Definition: wait.h:33
void mark_start()
Mark start of loop.
Definition: wait.cpp:68
void wait()
Wait until minimum loop time has been reached.
Definition: wait.cpp:78
A class for handling time.
Definition: time.h:93
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36