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