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