Fawkes API  Fawkes Development Version
direct_com_thread.cpp
1 
2 /***************************************************************************
3  * direct_com_thread.cpp - Robotino com thread for direct communication
4  *
5  * Created: Mon Apr 04 11:48:36 2016
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 "direct_com_thread.h"
23 #include "direct_com_message.h"
24 #include <baseapp/run.h>
25 #include <core/threading/mutex.h>
26 #include <core/threading/mutex_locker.h>
27 #include <utils/math/angle.h>
28 #include <utils/time/wait.h>
29 #include <tf/types.h>
30 
31 #include <interfaces/BatteryInterface.h>
32 #include <interfaces/RobotinoSensorInterface.h>
33 #include <interfaces/IMUInterface.h>
34 
35 #include <unistd.h>
36 
37 #include <libudev.h>
38 #include <boost/bind.hpp>
39 #include <boost/lambda/bind.hpp>
40 #include <boost/lambda/lambda.hpp>
41 #include <boost/filesystem.hpp>
42 
43 using namespace fawkes;
44 
45 /** @class DirectRobotinoComThread "openrobotino_com_thread.h"
46  * Thread to communicate with Robotino via OpenRobotino API (v1 or v2).
47  * @author Tim Niemueller
48  */
49 
50 /** Constructor. */
52  : RobotinoComThread("DirectRobotinoComThread"),
53  serial_(io_service_), io_service_work_(io_service_), deadline_(io_service_),
54  request_timer_(io_service_), nodata_timer_(io_service_), drive_timer_(io_service_)
55 {
57 }
58 
59 
60 /** Destructor. */
62 {
63 }
64 
65 
66 void
68 {
69  cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
70  cfg_sensor_update_cycle_time_ =
71  config->get_uint("/hardware/robotino/cycle-time");
72  cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
73  cfg_rpm_max_ = config->get_float("/hardware/robotino/motor/rpm-max");
74  cfg_nodata_timeout_ = config->get_uint("/hardware/robotino/direct/no-data-timeout");
75  cfg_drive_update_interval_ = config->get_uint("/hardware/robotino/direct/drive-update-interval");
76  cfg_read_timeout_ = config->get_uint("/hardware/robotino/direct/read-timeout");
77  cfg_log_checksum_errors_ = config->get_bool("/hardware/robotino/direct/checksums/log-errors");
78  cfg_checksum_error_recover_ = config->get_uint("/hardware/robotino/direct/checksums/recover-bound");
79  cfg_checksum_error_critical_ = config->get_uint("/hardware/robotino/direct/checksums/critical-bound");
80 
81  // -------------------------------------------------------------------------- //
82 
83  if (find_controld3()) {
84  throw Exception("Found running controld3, stop using 'sudo initctl stop controld3'");
85  }
86 
87  try {
88  cfg_device_ = config->get_string(("/hardware/robotino/direct/device"));
89  } catch (Exception &e) {
90 #ifdef HAVE_LIBUDEV
91  cfg_device_ = find_device_udev();
92 #else
93  throw Exception("No udev support, must configure serial device");
94 #endif
95  }
96 
97  deadline_.expires_at(boost::posix_time::pos_infin);
98  check_deadline();
99 
100  request_timer_.expires_from_now(boost::posix_time::milliseconds(-1));
101  drive_timer_.expires_at(boost::posix_time::pos_infin);
102 
103  digital_outputs_ = 0;
104 
105  open_device(/* wait for replies */ true);
106  open_tries_ = 0;
107  checksum_errors_ = 0;
108 
109  { // Disable all digital outputs initially
110  DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_SET_ALL_DIGITAL_OUTPUTS);
111  req.add_uint8(digital_outputs_);
112  send_message(req);
113  }
114 }
115 
116 
117 bool
119 {
120  //logger->log_info(name(), "Prepare Finalize");
121  request_timer_.cancel();
122  nodata_timer_.cancel();
123  drive_timer_.cancel();
124  drive_timer_.expires_at(boost::posix_time::pos_infin);
125  request_timer_.expires_at(boost::posix_time::pos_infin);
126  nodata_timer_.expires_at(boost::posix_time::pos_infin);
127  deadline_.expires_at(boost::posix_time::pos_infin);
128 
129  serial_.cancel();
130 
131  return true;
132 }
133 
134 void
136 {
137  close_device();
138 }
139 
140 void
142 {
143  reset_odometry();
144  request_data();
145  update_nodata_timer();
146 }
147 
148 void
150 {
151  if (finalize_prepared) {
152  usleep(1000);
153  return;
154  }
155 
156  if (opened_) {
157  try {
158  DirectRobotinoComMessage::pointer m = read_packet();
159  checksum_errors_ = 0;
160  process_message(m);
161  update_nodata_timer();
163  input_buffer_.consume(input_buffer_.size());
164  if (! finalize_prepared && opened_) {
165  checksum_errors_ += 1;
166  if (cfg_log_checksum_errors_) {
167  logger->log_warn(name(), "%s [%u consecutive errors]",
168  ce.what_no_backtrace(), checksum_errors_);
169  }
170  if (checksum_errors_ >= cfg_checksum_error_recover_) {
171  logger->log_warn(name(), "Large number of consecutive checksum errors, trying recover");
172  input_buffer_.consume(input_buffer_.size());
173  try {
174  DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
175  send_message(req);
176  request_data();
177  } catch (Exception &e) {}
178  } else if (checksum_errors_ >= cfg_checksum_error_critical_) {
179  logger->log_error(name(), "Critical number of consecutive checksum errors, re-opening");
180  input_buffer_.consume(input_buffer_.size());
181  close_device();
182  }
183  }
184  } catch (Exception &e) {
185  if (! finalize_prepared) {
186  if (opened_) {
187  logger->log_warn(name(), "Transmission error, sending ping");
188  logger->log_warn(name(), e);
189  input_buffer_.consume(input_buffer_.size());
190  try {
191  DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
192  send_message(req);
193  request_data();
194  } catch (Exception &e) {}
195  } else {
196  logger->log_warn(name(), "Transmission error, connection closed");
197  }
198  }
199  }
200  } else {
201  try {
202  logger->log_info(name(), "Re-opening device");
203  open_device(/* wait for replies */ false);
204  logger->log_info(name(), "Connection re-established after %u tries", open_tries_ + 1);
205  open_tries_ = 0;
206  request_data();
207  } catch (Exception &e) {
208  logger->log_warn(name(), "Re-open failed: %s", e.what_no_backtrace());
209  open_tries_ += 1;
210  if (open_tries_ >= (1000 / cfg_sensor_update_cycle_time_)) {
211  logger->log_error(name(), "Connection problem to base persists");
212  open_tries_ = 0;
213  }
214  }
215  }
216 }
217 
218 
219 void
220 DirectRobotinoComThread::process_message(DirectRobotinoComMessage::pointer m)
221 {
222  bool new_data = false;
223 
224  DirectRobotinoComMessage::command_id_t msgid;
225  while ((msgid = m->next_command()) != DirectRobotinoComMessage::CMDID_NONE) {
226  //logger->log_info(name(), "Command length: %u", m->command_length());
227 
228  if (msgid == DirectRobotinoComMessage::CMDID_ALL_MOTOR_READINGS) {
229  // there are four motors, one of which might be a gripper, therefore skips
230 
231  for (int i = 0; i < 3; ++i) data_.mot_velocity[i] = m->get_int16();
232  m->skip_int16();
233 
234  for (int i = 0; i < 3; ++i) data_.mot_position[i] = m->get_int32();
235  m->skip_int32();
236 
237  for (int i = 0; i < 3; ++i) data_.mot_current[i] = m->get_float();
238  new_data = true;
239 
240  } else if (msgid == DirectRobotinoComMessage::CMDID_DISTANCE_SENSOR_READINGS) {
241  for (int i = 0; i < 9; ++i) data_.ir_voltages[i] = m->get_float();
242  new_data = true;
243 
244  } else if (msgid == DirectRobotinoComMessage::CMDID_ALL_ANALOG_INPUTS) {
245  for (int i = 0; i < 8; ++i) data_.analog_in[i] = m->get_float();
246  new_data = true;
247 
248  } else if (msgid == DirectRobotinoComMessage::CMDID_ALL_DIGITAL_INPUTS) {
249  uint8_t value = m->get_uint8();
250  for (int i = 0; i < 8; ++i) data_.digital_in[i] = (value & (1 << i)) ? true : false;
251  new_data = true;
252 
253  } else if (msgid == DirectRobotinoComMessage::CMDID_BUMPER) {
254  data_.bumper = (m->get_uint8() != 0) ? true : false;
255  new_data = true;
256 
257  } else if (msgid == DirectRobotinoComMessage::CMDID_ODOMETRY) {
258  data_.odo_x = m->get_float();
259  data_.odo_y = m->get_float();
260  data_.odo_phi = m->get_float();
261  new_data = true;
262 
263  } else if (msgid == DirectRobotinoComMessage::CMDID_POWER_SOURCE_READINGS) {
264  float voltage = m->get_float();
265  float current = m->get_float();
266 
267  data_.bat_voltage = voltage * 1000.; // V -> mV
268  data_.bat_current = current * 1000.; // A -> mA
269 
270  // 22.0V is empty, 24.5V is full, this is just a guess
271  float soc = (voltage - 22.0f) / 2.5f;
272  soc = std::min(1.f, std::max(0.f, soc));
273  data_.bat_absolute_soc = soc;
274 
275  new_data = true;
276 
277  } else if (msgid == DirectRobotinoComMessage::CMDID_CHARGER_ERROR) {
278  uint8_t id = m->get_uint8();
279  uint32_t mtime = m->get_uint32();
280  std::string error = m->get_string();
281  logger->log_warn(name(), "Charger error (ID %u, Time: %u): %s",
282  id, mtime, error.c_str());
283  }
284  }
285 
286  if (new_data) {
287  MutexLocker lock(data_mutex_);
288  new_data_ = true;
289  data_.seq += 1;
290  data_.time.stamp();
291  }
292 }
293 
294 
295 void
297 {
298  try {
299  DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_ODOMETRY);
300  m.add_float(0.); // X (m)
301  m.add_float(0.); // Y (m)
302  m.add_float(0.); // rot (rad)
303  send_message(m);
304  } catch (Exception &e) {
305  logger->log_error(name(), "Resetting odometry failed, exception follows");
306  logger->log_error(name(), e);
307  }
308 }
309 
310 
311 void
312 DirectRobotinoComThread::set_motor_accel_limits(float min_accel, float max_accel)
313 {
314  try {
316  for (int i = 0; i < 2; ++i) {
317  req.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_ACCEL_LIMITS);
318  req.add_uint8(i);
319  req.add_float(min_accel);
320  req.add_float(max_accel);
321  }
322  send_message(req);
323  } catch (Exception &e) {
324  logger->log_error(name(), "Setting motor accel limits failed, exception follows");
325  logger->log_error(name(), e);
326  }
327 }
328 
329 void
330 DirectRobotinoComThread::set_digital_output(unsigned int digital_out, bool enable)
331 {
332  if (digital_out < 1 || digital_out > 8) {
333  throw Exception("Invalid digital output, must be in range [1..8], got %u",
334  digital_out);
335  }
336 
337  unsigned int digital_out_idx = digital_out - 1;
338 
339  if (enable) {
340  digital_outputs_ |= (1 << digital_out_idx);
341  } else {
342  digital_outputs_ &= ~(1 << digital_out_idx);
343  }
344 
345  try {
346  DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_SET_ALL_DIGITAL_OUTPUTS);
347  req.add_uint8(digital_outputs_);
348  send_message(req);
349  } catch (Exception &e) {
350  logger->log_error(name(), "Setting digital outputs failed, exception follows");
351  logger->log_error(name(), e);
352  }
353 
354  MutexLocker lock(data_mutex_);
355  for (int i = 0; i < 8; ++i) data_.digital_out[i] = (digital_outputs_ & (1 << i)) ? true : false;
356  new_data_ = true;
357 }
358 
359 
360 bool
362 {
363  return serial_.is_open();
364 }
365 
366 
367 void
368 DirectRobotinoComThread::get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)
369 {
370  MutexLocker lock(data_mutex_);
371  a1 = data_.mot_velocity[0];
372  a2 = data_.mot_velocity[1];
373  a3 = data_.mot_velocity[2];
374 
375  seq = data_.seq;
376  t = data_.time;
377 }
378 
379 
380 void
381 DirectRobotinoComThread::get_odometry(double &x, double &y, double &phi)
382 {
383  MutexLocker lock(data_mutex_);
384  x = data_.odo_x;
385  y = data_.odo_y;
386  phi = data_.odo_phi;
387 }
388 
389 bool
391 {
392  MutexLocker lock(data_mutex_);
393  return false;
394 }
395 
396 void
397 DirectRobotinoComThread::set_speed_points(float s1, float s2, float s3)
398 {
399  float bounded_s1 = std::max(-cfg_rpm_max_, std::min(cfg_rpm_max_, s1));
400  float bounded_s2 = std::max(-cfg_rpm_max_, std::min(cfg_rpm_max_, s2));
401  float bounded_s3 = std::max(-cfg_rpm_max_, std::min(cfg_rpm_max_, s3));
402 
403  try {
405  m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
406  m.add_uint8(0);
407  m.add_uint16((uint16_t)roundf(bounded_s1));
408  m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
409  m.add_uint8(1);
410  m.add_uint16((uint16_t)roundf(bounded_s2));
411  m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
412  m.add_uint8(2);
413  m.add_uint16((uint16_t)roundf(bounded_s3));
414  send_message(m);
415  } catch (Exception &e) {
416  logger->log_error(name(), "Setting speed points failed, exception follows");
417  logger->log_error(name(), e);
418  }
419 }
420 
421 void
423 {
424 }
425 
426 void
428 {
429  try {
430  DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_EMERGENCY_BUMPER);
431  m.add_uint8(enabled ? 1 : 0);
432  send_message(m);
433 
434  MutexLocker lock(data_mutex_);
435  data_.bumper_estop_enabled = enabled;
436  } catch (Exception &e) {
437  logger->log_error(name(), "Setting bumper estop state failed, exception follows");
438  logger->log_error(name(), e);
439  }
440 }
441 
442 
443 std::string
444 DirectRobotinoComThread::find_device_udev()
445 {
446  std::string cfg_device = "";
447 
448  // try to find device using udev
449  struct udev *udev;
450  struct udev_enumerate *enumerate;
451  struct udev_list_entry *devices, *dev_list_entry;
452  struct udev_device *dev, *usb_device;
453  udev = udev_new();
454  if (! udev) {
455  throw Exception("RobotinoDirect: Failed to initialize udev for "
456  "device detection");
457  }
458 
459  enumerate = udev_enumerate_new(udev);
460  udev_enumerate_add_match_subsystem(enumerate, "tty");
461  udev_enumerate_scan_devices(enumerate);
462 
463  devices = udev_enumerate_get_list_entry(enumerate);
464  udev_list_entry_foreach(dev_list_entry, devices) {
465  const char *path;
466 
467  path = udev_list_entry_get_name(dev_list_entry);
468  if (! path) continue;
469 
470  dev = udev_device_new_from_syspath(udev, path);
471  usb_device = udev_device_get_parent_with_subsystem_devtype(dev, "usb",
472  "usb_device");
473  if (! dev || ! usb_device) continue;
474 
475  std::string vendor_id = udev_device_get_property_value(dev, "ID_VENDOR_ID");
476  std::string model_id = udev_device_get_property_value(dev, "ID_MODEL_ID");
477 
478  if (vendor_id == "1e29" && model_id == "040d") {
479  // found Robotino 3 device
480  cfg_device = udev_device_get_property_value(dev, "DEVNAME");
481 
482  /*
483  struct udev_list_entry * props =
484  udev_device_get_properties_list_entry(dev);
485  udev_list_entry *p;
486  udev_list_entry_foreach(p, props)
487  {
488  printf(" %s = %s\n", udev_list_entry_get_name(p), udev_list_entry_get_value(p));
489  }
490  */
491 
492  std::string vendor = udev_device_get_property_value(dev, "ID_VENDOR_FROM_DATABASE");
493  std::string model = "unknown";
494  const char *model_from_db = udev_device_get_property_value(dev, "ID_MODEL_FROM_DATABASE");
495  if (model_from_db) {
496  model = model_from_db;
497  } else {
498  model = udev_device_get_property_value(dev, "ID_MODEL");
499  }
500  logger->log_debug(name(), "Found %s %s at device %s",
501  vendor.c_str(), model.c_str(), cfg_device.c_str());
502  break;
503  }
504  }
505 
506  udev_enumerate_unref(enumerate);
507  udev_unref(udev);
508 
509  if (cfg_device == "") {
510  throw Exception("No robotino device was found");
511  }
512 
513  return cfg_device;
514 }
515 
516 
517 bool
518 DirectRobotinoComThread::find_controld3()
519 {
520  bool rv = false;
521  boost::filesystem::path p("/proc");
522 
523  using namespace boost::filesystem;
524 
525  try {
526  if (boost::filesystem::exists(p) && boost::filesystem::is_directory(p)) {
527 
528  directory_iterator di;
529  for (di = directory_iterator(p); di != directory_iterator(); ++di) {
530  directory_entry &d = *di;
531  //for (directory_entry &d : directory_iterator(p))
532  std::string f = d.path().filename().native();
533  bool is_process = true;
534  for (std::string::size_type i = 0; i < f.length(); ++i) {
535  if (! isdigit(f[i])) {
536  is_process = false;
537  break;
538  }
539  }
540  if (is_process) {
541  path pproc(d.path());
542  pproc /= "stat";
543 
544  FILE *f = fopen(pproc.c_str(), "r");
545  if (f) {
546  int pid;
547  char *procname;
548  if (fscanf(f, "%d (%m[a-z0-9])", &pid, &procname) == 2) {
549 
550  if (strcmp("controld3", procname) == 0) {
551  rv = true;
552  }
553  ::free(procname);
554  }
555  fclose(f);
556  }
557  }
558  }
559 
560  } else {
561  logger->log_warn(name(), "Cannot open /proc, cannot determine if controld3 is running");
562  return false;
563  }
564 
565  } catch (const boost::filesystem::filesystem_error &ex) {
566  logger->log_warn(name(), "Failure to determine if controld3 is running: %s", ex.what());
567  return false;
568  }
569 
570  return rv;
571 }
572 
573 void
574 DirectRobotinoComThread::open_device(bool wait_replies)
575 {
576  if (finalize_prepared) return;
577 
578  try {
579  input_buffer_.consume(input_buffer_.size());
580 
581  boost::mutex::scoped_lock lock(io_mutex_);
582 
583  serial_.open(cfg_device_);
584  //serial_.set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::none));
585  serial_.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
586  serial_.set_option(boost::asio::serial_port::baud_rate(115200));
587 
588  opened_ = true;
589  } catch (boost::system::system_error &e) {
590  throw Exception("RobotinoDirect failed I/O: %s", e.what());
591  }
592 
593  {
595  req.add_command(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
596  req.add_command(DirectRobotinoComMessage::CMDID_GET_SW_VERSION);
597 
598  if (wait_replies) {
599  DirectRobotinoComMessage::pointer m = send_and_recv(req);
600 
601  //logger->log_info(name(), "Escaped:\n%s", m->to_string(true).c_str());
602  //logger->log_info(name(), "Un-Escaped:\n%s", m->to_string(false).c_str());
603  std::string hw_version, sw_version;
604  DirectRobotinoComMessage::command_id_t msgid;
605  while ((msgid = m->next_command()) != DirectRobotinoComMessage::CMDID_NONE) {
606  if (msgid == DirectRobotinoComMessage::CMDID_HW_VERSION) {
607  hw_version = m->get_string();
608  } else if (msgid == DirectRobotinoComMessage::CMDID_SW_VERSION) {
609  sw_version = m->get_string();
610 
611  } else if (msgid == DirectRobotinoComMessage::CMDID_CHARGER_ERROR) {
612  uint8_t id = m->get_uint8();
613  uint32_t mtime = m->get_uint32();
614  std::string error = m->get_string();
615  logger->log_warn(name(), "Charger error (ID %u, Time: %u): %s",
616  id, mtime, error.c_str());
617  //} else {
618  //logger->log_debug(name(), " - %u\n", msgid);
619  }
620  }
621  if (hw_version.empty() || sw_version.empty()) {
622  close_device();
623  throw Exception("RobotinoDirect: no reply to version inquiry from robot");
624  }
625  logger->log_debug(name(), "Connected, HW Version: %s SW Version: %s",
626  hw_version.c_str(), sw_version.c_str());
627  } else {
628  try {
629  send_message(req);
630  } catch (Exception &e) {
631  logger->log_error(name(), "Requesting version information failed, exception follows");
632  logger->log_error(name(), e);
633  }
634  }
635  }
636 }
637 
638 
639 void
640 DirectRobotinoComThread::close_device()
641 {
642  serial_.cancel();
643  serial_.close();
644  opened_ = false;
645  open_tries_ = 0;
646 }
647 
648 
649 void
650 DirectRobotinoComThread::flush_device()
651 {
652  if (serial_.is_open()) {
653  try {
654  boost::system::error_code ec = boost::asio::error::would_block;
655  size_t bytes_read = 0;
656  do {
657  ec = boost::asio::error::would_block;
658  bytes_read = 0;
659 
660  deadline_.expires_from_now(boost::posix_time::milliseconds(cfg_read_timeout_));
661  boost::asio::async_read(serial_, input_buffer_,
662  boost::asio::transfer_at_least(1),
663  (boost::lambda::var(ec) = boost::lambda::_1,
664  boost::lambda::var(bytes_read) = boost::lambda::_2));
665 
666  do io_service_.run_one(); while (ec == boost::asio::error::would_block);
667 
668  if (bytes_read > 0) {
669  logger->log_warn(name(), "Flushing %zu bytes\n", bytes_read);
670  }
671 
672  } while (bytes_read > 0);
673  deadline_.expires_from_now(boost::posix_time::pos_infin);
674  } catch (boost::system::system_error &e) {
675  // ignore, just assume done, if there really is an error we'll
676  // catch it later on
677  }
678  }
679 }
680 
681 void
682 DirectRobotinoComThread::send_message(DirectRobotinoComMessage &msg)
683 {
684  boost::mutex::scoped_lock lock(io_mutex_);
685  if (opened_) {
686  //logger->log_warn(name(), "Sending");
687  boost::system::error_code ec;
688  boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()), ec);
689 
690  if (ec) {
691  close_device();
692  throw Exception("Error while writing message (%s), closing connection",
693  ec.message().c_str());
694  }
695  }
696 }
697 
698 std::shared_ptr<DirectRobotinoComMessage>
699 DirectRobotinoComThread::send_and_recv(DirectRobotinoComMessage &msg)
700 {
701  boost::mutex::scoped_lock lock(io_mutex_);
702  if (opened_) {
703  boost::system::error_code ec;
704  boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()), ec);
705  if (ec) {
706  logger->log_error(name(), "Error while writing message (%s), closing connection",
707  ec.message().c_str());
708 
709  close_device();
710  throw Exception("RobotinoDirect: write failed (%s)", ec.message().c_str());
711  }
712  std::shared_ptr<DirectRobotinoComMessage> m = read_packet();
713  return m;
714  } else {
715  throw Exception("RobotinoDirect: serial device not opened");
716  }
717 }
718 
719 /// @cond INTERNAL
720 /** Matcher to count unescaped number of bytes. */
721 class match_unescaped_length
722 {
723 public:
724  explicit match_unescaped_length(unsigned short length) : length_(length), l_(0) {}
725 
726  template <typename Iterator>
727  std::pair<Iterator, bool> operator()(
728  Iterator begin, Iterator end) const
729  {
730  Iterator i = begin;
731  while (i != end && l_ < length_) {
732  if (*i++ != DirectRobotinoComMessage::MSG_DATA_ESCAPE) {
733  l_ += 1;
734  }
735  }
736  return std::make_pair(i, (l_ == length_));
737  }
738 
739 private:
740  unsigned short length_;
741  mutable unsigned short l_;
742 };
743 
744 namespace boost {
745  namespace asio {
746  template <> struct is_match_condition<match_unescaped_length>
747  : public boost::true_type {};
748  } // namespace asio
749 }
750 /// @endcond
751 
753 DirectRobotinoComThread::read_packet()
754 {
755  boost::system::error_code ec = boost::asio::error::would_block;
756  size_t bytes_read = 0;
757 
758  boost::asio::async_read_until(serial_, input_buffer_, DirectRobotinoComMessage::MSG_HEAD,
759  (boost::lambda::var(ec) = boost::lambda::_1,
760  boost::lambda::var(bytes_read) = boost::lambda::_2));
761 
762  do io_service_.run_one(); while (ec == boost::asio::error::would_block);
763 
764  if (ec) {
765  if (ec.value() == boost::system::errc::operation_canceled) {
766  throw Exception("Timeout (1) on initial synchronization");
767  } else {
768  throw Exception("Error (1) on initial synchronization: %s", ec.message().c_str());
769  }
770  }
771 
772  // Read all potential junk before the start header
773  // if (bytes_read > 1) {
774  // logger->log_warn(name(), "Read junk off line");
775  // }
776  input_buffer_.consume(bytes_read - 1);
777 
778  // start timeout for remaining packet
779  deadline_.expires_from_now(boost::posix_time::milliseconds(cfg_read_timeout_));
780 
781  // read packet length
782  ec = boost::asio::error::would_block;
783  bytes_read = 0;
784  boost::asio::async_read_until(serial_, input_buffer_,
785  match_unescaped_length(2),
786  (boost::lambda::var(ec) = boost::lambda::_1,
787  boost::lambda::var(bytes_read) = boost::lambda::_2));
788 
789  do io_service_.run_one(); while (ec == boost::asio::error::would_block);
790 
791  if (ec) {
792  if (ec.value() == boost::system::errc::operation_canceled) {
793  throw Exception("Timeout (2) on initial synchronization");
794  } else {
795  throw Exception("Error (2) on initial synchronization: %s", ec.message().c_str());
796  }
797  }
798 
799  const unsigned char *length_escaped =
800  boost::asio::buffer_cast<const unsigned char*>(input_buffer_.data());
801 
802  unsigned char length_unescaped[2];
803  DirectRobotinoComMessage::unescape(length_unescaped, 2, &length_escaped[1], bytes_read);
804 
805  unsigned short length =
806  DirectRobotinoComMessage::parse_uint16(length_unescaped);
807 
808  // read remaining packet
809  ec = boost::asio::error::would_block;
810  bytes_read = 0;
811  boost::asio::async_read_until(serial_, input_buffer_,
812  match_unescaped_length(length + 2), // +2: checksum
813  (boost::lambda::var(ec) = boost::lambda::_1,
814  boost::lambda::var(bytes_read) = boost::lambda::_2));
815 
816  do io_service_.run_one(); while (ec == boost::asio::error::would_block);
817 
818  if (ec) {
819  if (ec.value() == boost::system::errc::operation_canceled) {
820  throw Exception("Timeout (3) on initial synchronization (reading %u bytes, have %zu)",
821  length, input_buffer_.size());
822  } else {
823  throw Exception("Error (3) on initial synchronization: %s", ec.message().c_str());
824  }
825  }
826 
827  deadline_.expires_at(boost::posix_time::pos_infin);
828 
829  DirectRobotinoComMessage::pointer m = std::make_shared<DirectRobotinoComMessage>
830  (boost::asio::buffer_cast<const unsigned char*> (input_buffer_.data()), input_buffer_.size());
831 
832  input_buffer_.consume(m->escaped_data_size());
833 
834  return m;
835 }
836 
837 
838 /** Check whether the deadline has passed.
839  * We compare the deadline against
840  * the current time since a new asynchronous operation may have moved the
841  * deadline before this actor had a chance to run.
842  */
843 void
844 DirectRobotinoComThread::check_deadline()
845 {
846  if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) {
847  serial_.cancel();
848  deadline_.expires_at(boost::posix_time::pos_infin);
849  }
850 
851  deadline_.async_wait(boost::lambda::bind(&DirectRobotinoComThread::check_deadline, this));
852 }
853 
854 
855 /** Request new data.
856  */
857 void
858 DirectRobotinoComThread::request_data()
859 {
860  if (finalize_prepared) return;
861 
862  if (request_timer_.expires_from_now() < boost::posix_time::milliseconds(0)) {
863  request_timer_.expires_from_now(boost::posix_time::milliseconds(cfg_sensor_update_cycle_time_));
864  request_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_request_data, this,
865  boost::asio::placeholders::error));
866  }
867 }
868 
869 void
870 DirectRobotinoComThread::handle_request_data(const boost::system::error_code &ec)
871 {
872  if (! ec) {
874  req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_MOTOR_READINGS);
875  req.add_command(DirectRobotinoComMessage::CMDID_GET_DISTANCE_SENSOR_READINGS);
876  req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_ANALOG_INPUTS);
877  req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_DIGITAL_INPUTS);
878  req.add_command(DirectRobotinoComMessage::CMDID_GET_BUMPER);
879  req.add_command(DirectRobotinoComMessage::CMDID_GET_ODOMETRY);
880  req.add_command(DirectRobotinoComMessage::CMDID_GET_GYRO_Z_ANGLE);
881  // This command is documented in the wiki but does not exist in the
882  // enum and is not handled in the firmware. Instead, the information
883  // is sent with every reply, so we also get it here automatically.
884  // This has been checked in Robotino3 firmware 1.1.1
885  // req.add_command(DirectRobotinoComMessage::CMDID_GET_POWER_SOURCE_READINGS);
886 
887  //req.pack();
888  //logger->log_debug(name(), "Req1:\n%s", req.to_string().c_str());
889 
890  try {
891  send_message(req);
892  } catch (Exception &e) {
893  logger->log_warn(name(), "Sending message failed, excpeption follows");
894  logger->log_warn(name(), e);
895  }
896 
897  } else {
898  logger->log_warn(name(), "Request timer failed: %s", ec.message().c_str());
899  }
900 
901  if (! finalize_prepared && opened_) {
902  request_data();
903  }
904 }
905 
906 void
907 DirectRobotinoComThread::set_desired_vel(float vx, float vy, float omega)
908 {
909  RobotinoComThread::set_desired_vel(vx, vy, omega);
910  drive();
911 }
912 
913 
914 void
915 DirectRobotinoComThread::drive()
916 {
917  if (finalize_prepared) return;
918 
919  drive_timer_.expires_from_now(boost::posix_time::milliseconds(cfg_drive_update_interval_));
920  drive_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_drive, this,
921  boost::asio::placeholders::error));
922 }
923 
924 
925 void
926 DirectRobotinoComThread::handle_drive(const boost::system::error_code &ec)
927 {
928  if (! ec) {
929  if (update_velocities()) drive();
930  }
931 }
932 
933 
934 void
935 DirectRobotinoComThread::update_nodata_timer()
936 {
937  nodata_timer_.cancel();
938  nodata_timer_.expires_from_now(boost::posix_time::milliseconds(cfg_nodata_timeout_));
939  nodata_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_nodata, this,
940  boost::asio::placeholders::error));
941 }
942 
943 void
944 DirectRobotinoComThread::handle_nodata(const boost::system::error_code &ec)
945 {
946  // ec may be set if the timer is cancelled, i.e., updated
947  if (! ec) {
948  logger->log_error(name(), "No data received for too long, re-establishing connection");
949  close_device();
950  }
951 }
SensorData data_
Data struct that must be updated whenever new data is available.
Definition: com_thread.h:116
virtual void finalize()
Finalize the thread.
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
Definition: com_thread.h:118
void add_uint16(uint16_t value)
Add 16-bit unsigned integer to current command.
void add_command(command_id_t cmdid)
Add a command header.
virtual void set_gripper(bool opened)
Open or close gripper.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void add_uint8(uint8_t value)
Add 8-bit unsigned integer to current command.
bool finalize_prepared
True if prepare_finalize() has been called and was not stopped with a cancel_finalize(), false otherwise.
Definition: thread.h:138
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
Mutex locking helper.
Definition: mutex_locker.h:33
virtual void loop()
Code to execute in the thread.
bool update_velocities()
Update velocity values.
Definition: com_thread.cpp:217
A class for handling time.
Definition: time.h:91
virtual void set_bumper_estop_enabled(bool enabled)
Enable or disable emergency stop on bumper contact.
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
Definition: thread.cpp:727
std::shared_ptr< DirectRobotinoComMessage > pointer
shared pointer to direct com message
boost::asio::const_buffer buffer()
Get access to buffer for sending.
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
virtual void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
Definition: com_thread.cpp:204
Robotino communication message.
virtual void once()
Execute an action exactly once.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual bool is_gripper_open()
Check if gripper is open.
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 init()
Initialize the thread.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:686
virtual void reset_odometry()
Reset odometry to zero.
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 set_digital_output(unsigned int digital_out, bool enable)
Set digital output state.
static uint16_t parse_uint16(const unsigned char *buf)
Parse 16-bit unsigned integer from given buffer.
static size_t unescape(unsigned char *unescaped, size_t unescaped_size, const unsigned char *escaped, size_t escaped_size)
Unescape a number of unescaped bytes.
bool prepare_finalize_user()
Prepare finalization user implementation.
virtual void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual bool is_connected()
Check if we are connected to OpenRobotino.
virtual void set_speed_points(float s1, float s2, float s3)
Set speed points for wheels.
DirectRobotinoComThread()
Constructor.
virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)
Get actual velocity.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual ~DirectRobotinoComThread()
Destructor.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual void set_motor_accel_limits(float min_accel, float max_accel)
Set acceleration limits of motors.
virtual void get_odometry(double &x, double &y, double &phi)
Get latest odometry value.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
void add_float(float value)
Add float to current command.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Excpetion thrown on checksum errors.