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
43using 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
68void
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
119bool
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
136void
138{
139 close_device();
140}
141
142void
144{
146 request_data();
147 update_nodata_timer();
148}
149
150void
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_) {
170 "%s [%u consecutive errors]",
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
224void
225DirectRobotinoComThread::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) {
298 new_data_ = true;
299 data_.seq += 1;
300 data_.time.stamp();
301 }
302}
303
304void
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
319void
320DirectRobotinoComThread::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
337void
338DirectRobotinoComThread::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
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
367bool
369{
370 return serial_.is_open();
371}
372
373void
375 float & a2,
376 float & a3,
377 unsigned int &seq,
378 fawkes::Time &t)
379{
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
389void
390DirectRobotinoComThread::get_odometry(double &x, double &y, double &phi)
391{
393 x = data_.odo_x;
394 y = data_.odo_y;
395 phi = data_.odo_phi;
396}
397
398bool
400{
402 return false;
403}
404
405void
406DirectRobotinoComThread::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
430void
432{
433}
434
435void
437{
438 try {
439 DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_EMERGENCY_BUMPER);
440 m.add_uint8(enabled ? 1 : 0);
441 send_message(m);
442
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
451std::string
452DirectRobotinoComThread::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 }
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
526bool
527DirectRobotinoComThread::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
580void
581DirectRobotinoComThread::open_device(bool wait_replies)
582{
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 }
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
647void
648DirectRobotinoComThread::close_device()
649{
650 serial_.cancel();
651 serial_.close();
652 opened_ = false;
653 open_tries_ = 0;
654}
655
656void
657DirectRobotinoComThread::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
691void
692DirectRobotinoComThread::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
707std::shared_ptr<DirectRobotinoComMessage>
708DirectRobotinoComThread::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) {
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. */
731class match_unescaped_length
732{
733public:
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
751private:
752 unsigned short length_;
753 mutable unsigned short l_;
754};
755
756namespace boost {
757namespace asio {
758template <>
759struct is_match_condition<match_unescaped_length> : public boost::true_type
760{
761};
762} // namespace asio
763} // namespace boost
764/// @endcond
765
767DirectRobotinoComThread::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 */
866void
867DirectRobotinoComThread::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 */
879void
880DirectRobotinoComThread::request_data()
881{
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
893void
894DirectRobotinoComThread::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
930void
931DirectRobotinoComThread::set_desired_vel(float vx, float vy, float omega)
932{
934 drive();
935}
936
937void
938DirectRobotinoComThread::drive()
939{
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
948void
949DirectRobotinoComThread::handle_drive(const boost::system::error_code &ec)
950{
951 if (!ec) {
952 if (update_velocities())
953 drive();
954 }
955}
956
957void
958DirectRobotinoComThread::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
966void
967DirectRobotinoComThread::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.