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
49using 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
69void
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
137void
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
168void
170{
172}
173
174void
176{
177 time_wait_->mark_start();
178
179 if (com_->isConnected()) {
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
208void
209OpenRobotinoComThread::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
267void
268OpenRobotinoComThread::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) {
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) {
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) {
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) {
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. */
354void
355OpenRobotinoComThread::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. */
367void
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
382void
383OpenRobotinoComThread::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
389void
390OpenRobotinoComThread::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 */
398bool
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 */
411void
413 float & a2,
414 float & a3,
415 unsigned int &seq,
416 fawkes::Time &t)
417{
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 */
451void
452OpenRobotinoComThread::get_odometry(double &x, double &y, double &phi)
453{
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 */
474bool
476{
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 */
495void
496OpenRobotinoComThread::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 */
513void
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
528void
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