22 #include "direct_com_thread.h"
24 #include "direct_com_message.h"
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>
33 #include <utils/math/angle.h>
34 #include <utils/time/wait.h>
36 #include <boost/bind.hpp>
37 #include <boost/filesystem.hpp>
38 #include <boost/lambda/bind.hpp>
39 #include <boost/lambda/lambda.hpp>
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_)
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");
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");
86 if (find_controld3()) {
87 throw Exception(
"Found running controld3, stop using 'sudo initctl stop controld3'");
94 cfg_device_ = find_device_udev();
96 throw Exception(
"No udev support, must configure serial device");
100 deadline_.expires_at(boost::posix_time::pos_infin);
103 request_timer_.expires_from_now(boost::posix_time::milliseconds(-1));
104 drive_timer_.expires_at(boost::posix_time::pos_infin);
106 digital_outputs_ = 0;
110 checksum_errors_ = 0;
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);
147 update_nodata_timer();
161 checksum_errors_ = 0;
163 update_nodata_timer();
165 input_buffer_.consume(input_buffer_.size());
167 checksum_errors_ += 1;
168 if (cfg_log_checksum_errors_) {
170 "%s [%u consecutive errors]",
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());
183 }
else if (checksum_errors_ >= cfg_checksum_error_critical_) {
185 input_buffer_.consume(input_buffer_.size());
194 input_buffer_.consume(input_buffer_.size());
210 logger->
log_info(
name(),
"Connection re-established after %u tries", open_tries_ + 1);
216 if (open_tries_ >= (1000 / cfg_sensor_update_cycle_time_)) {
227 bool new_data =
false;
229 DirectRobotinoComMessage::command_id_t msgid;
230 while ((msgid = m->next_command()) != DirectRobotinoComMessage::CMDID_NONE) {
233 if (msgid == DirectRobotinoComMessage::CMDID_ALL_MOTOR_READINGS) {
236 for (
int i = 0; i < 3; ++i)
237 data_.mot_velocity[i] = m->get_int16();
240 for (
int i = 0; i < 3; ++i)
241 data_.mot_position[i] = m->get_int32();
244 for (
int i = 0; i < 3; ++i)
245 data_.mot_current[i] = m->get_float();
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();
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();
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;
264 }
else if (msgid == DirectRobotinoComMessage::CMDID_BUMPER) {
265 data_.bumper = (m->get_uint8() != 0) ?
true :
false;
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();
274 }
else if (msgid == DirectRobotinoComMessage::CMDID_POWER_SOURCE_READINGS) {
275 float voltage = m->get_float();
276 float current = m->get_float();
278 data_.bat_voltage = voltage * 1000.;
279 data_.bat_current = current * 1000.;
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;
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());
324 for (
int i = 0; i < 2; ++i) {
325 req.
add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_ACCEL_LIMITS);
340 if (digital_out < 1 || digital_out > 8) {
341 throw Exception(
"Invalid digital output, must be in range [1..8], got %u", digital_out);
344 unsigned int digital_out_idx = digital_out - 1;
347 digital_outputs_ |= (1 << digital_out_idx);
349 digital_outputs_ &= ~(1 << digital_out_idx);
362 for (
int i = 0; i < 8; ++i)
363 data_.digital_out[i] = (digital_outputs_ & (1 << i)) ? true :
false;
370 return serial_.is_open();
381 a1 =
data_.mot_velocity[0];
382 a2 =
data_.mot_velocity[1];
383 a3 =
data_.mot_velocity[2];
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));
414 m.
add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
417 m.
add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
420 m.
add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
444 data_.bumper_estop_enabled = enabled;
452 DirectRobotinoComThread::find_device_udev()
454 std::string cfg_device =
"";
458 struct udev_enumerate * enumerate;
459 struct udev_list_entry *devices, *dev_list_entry;
460 struct udev_device * dev, *usb_device;
463 throw Exception(
"RobotinoDirect: Failed to initialize udev for "
467 enumerate = udev_enumerate_new(udev);
468 udev_enumerate_add_match_subsystem(enumerate,
"tty");
469 udev_enumerate_scan_devices(enumerate);
471 devices = udev_enumerate_get_list_entry(enumerate);
472 udev_list_entry_foreach(dev_list_entry, devices)
476 path = udev_list_entry_get_name(dev_list_entry);
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)
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");
488 if (vendor_id ==
"1e29" && model_id ==
"040d") {
490 cfg_device = udev_device_get_property_value(dev,
"DEVNAME");
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");
506 model = model_from_db;
508 model = udev_device_get_property_value(dev,
"ID_MODEL");
511 name(),
"Found %s %s at device %s", vendor.c_str(), model.c_str(), cfg_device.c_str());
516 udev_enumerate_unref(enumerate);
519 if (cfg_device ==
"") {
520 throw Exception(
"No robotino device was found");
527 DirectRobotinoComThread::find_controld3()
530 boost::filesystem::path p(
"/proc");
532 using namespace boost::filesystem;
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;
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])) {
549 path pproc(d.path());
552 FILE *f = fopen(pproc.c_str(),
"r");
556 if (fscanf(f,
"%d (%m[a-z0-9])", &pid, &procname) == 2) {
557 if (strcmp(
"controld3", procname) == 0) {
568 logger->
log_warn(
name(),
"Cannot open /proc, cannot determine if controld3 is running");
572 }
catch (
const boost::filesystem::filesystem_error &ex) {
573 logger->
log_warn(
name(),
"Failure to determine if controld3 is running: %s", ex.what());
581 DirectRobotinoComThread::open_device(
bool wait_replies)
587 input_buffer_.consume(input_buffer_.size());
589 boost::mutex::scoped_lock lock(io_mutex_);
591 serial_.open(cfg_device_);
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));
597 }
catch (boost::system::system_error &e) {
598 throw Exception(
"RobotinoDirect failed I/O: %s", e.what());
603 req.
add_command(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
604 req.
add_command(DirectRobotinoComMessage::CMDID_GET_SW_VERSION);
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();
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());
628 if (hw_version.empty() || sw_version.empty()) {
630 throw Exception(
"RobotinoDirect: no reply to version inquiry from robot");
633 "Connected, HW Version: %s SW Version: %s",
648 DirectRobotinoComThread::close_device()
657 DirectRobotinoComThread::flush_device()
659 if (serial_.is_open()) {
661 boost::system::error_code ec = boost::asio::error::would_block;
662 size_t bytes_read = 0;
664 ec = boost::asio::error::would_block;
667 deadline_.expires_from_now(boost::posix_time::milliseconds(cfg_read_timeout_));
668 boost::asio::async_read(serial_,
670 boost::asio::transfer_at_least(1),
671 (boost::lambda::var(ec) = boost::lambda::_1,
672 boost::lambda::var(bytes_read) = boost::lambda::_2));
675 io_service_.run_one();
676 while (ec == boost::asio::error::would_block);
678 if (bytes_read > 0) {
682 }
while (bytes_read > 0);
683 deadline_.expires_from_now(boost::posix_time::pos_infin);
684 }
catch (boost::system::system_error &e) {
694 boost::mutex::scoped_lock lock(io_mutex_);
697 boost::system::error_code ec;
698 boost::asio::write(serial_, boost::asio::const_buffers_1(msg.
buffer()), ec);
702 throw Exception(
"Error while writing message (%s), closing connection", ec.message().c_str());
707 std::shared_ptr<DirectRobotinoComMessage>
710 boost::mutex::scoped_lock lock(io_mutex_);
712 boost::system::error_code ec;
713 boost::asio::write(serial_, boost::asio::const_buffers_1(msg.
buffer()), ec);
716 "Error while writing message (%s), closing connection",
717 ec.message().c_str());
720 throw Exception(
"RobotinoDirect: write failed (%s)", ec.message().c_str());
722 std::shared_ptr<DirectRobotinoComMessage> m = read_packet();
725 throw Exception(
"RobotinoDirect: serial device not opened");
731 class match_unescaped_length
734 explicit match_unescaped_length(
unsigned short length) : length_(length), l_(0)
738 template <
typename Iterator>
739 std::pair<Iterator, bool>
740 operator()(Iterator begin, Iterator end)
const
743 while (i != end && l_ < length_) {
744 if (*i++ != DirectRobotinoComMessage::MSG_DATA_ESCAPE) {
748 return std::make_pair(i, (l_ == length_));
752 unsigned short length_;
753 mutable unsigned short l_;
759 struct is_match_condition<match_unescaped_length> :
public boost::true_type
767 DirectRobotinoComThread::read_packet()
769 boost::system::error_code ec = boost::asio::error::would_block;
770 size_t bytes_read = 0;
772 boost::asio::async_read_until(serial_,
774 DirectRobotinoComMessage::MSG_HEAD,
775 (boost::lambda::var(ec) = boost::lambda::_1,
776 boost::lambda::var(bytes_read) = boost::lambda::_2));
779 io_service_.run_one();
780 while (ec == boost::asio::error::would_block);
783 if (ec.value() == boost::system::errc::operation_canceled) {
784 throw Exception(
"Timeout (1) on initial synchronization");
786 throw Exception(
"Error (1) on initial synchronization: %s", ec.message().c_str());
794 input_buffer_.consume(bytes_read - 1);
797 deadline_.expires_from_now(boost::posix_time::milliseconds(cfg_read_timeout_));
800 ec = boost::asio::error::would_block;
802 boost::asio::async_read_until(serial_,
804 match_unescaped_length(2),
805 (boost::lambda::var(ec) = boost::lambda::_1,
806 boost::lambda::var(bytes_read) = boost::lambda::_2));
809 io_service_.run_one();
810 while (ec == boost::asio::error::would_block);
813 if (ec.value() == boost::system::errc::operation_canceled) {
814 throw Exception(
"Timeout (2) on initial synchronization");
816 throw Exception(
"Error (2) on initial synchronization: %s", ec.message().c_str());
820 const unsigned char *length_escaped =
821 boost::asio::buffer_cast<const unsigned char *>(input_buffer_.data());
823 unsigned char length_unescaped[2];
829 ec = boost::asio::error::would_block;
831 boost::asio::async_read_until(serial_,
833 match_unescaped_length(length + 2),
834 (boost::lambda::var(ec) = boost::lambda::_1,
835 boost::lambda::var(bytes_read) = boost::lambda::_2));
838 io_service_.run_one();
839 while (ec == boost::asio::error::would_block);
842 if (ec.value() == boost::system::errc::operation_canceled) {
843 throw Exception(
"Timeout (3) on initial synchronization (reading %u bytes, have %zu)",
845 input_buffer_.size());
847 throw Exception(
"Error (3) on initial synchronization: %s", ec.message().c_str());
851 deadline_.expires_at(boost::posix_time::pos_infin);
854 boost::asio::buffer_cast<const unsigned char *>(input_buffer_.data()), input_buffer_.size());
856 input_buffer_.consume(m->escaped_data_size());
867 DirectRobotinoComThread::check_deadline()
869 if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) {
871 deadline_.expires_at(boost::posix_time::pos_infin);
874 deadline_.async_wait(boost::lambda::bind(&DirectRobotinoComThread::check_deadline,
this));
880 DirectRobotinoComThread::request_data()
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,
889 boost::asio::placeholders::error));
894 DirectRobotinoComThread::handle_request_data(
const boost::system::error_code &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);
938 DirectRobotinoComThread::drive()
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));
949 DirectRobotinoComThread::handle_drive(
const boost::system::error_code &ec)
958 DirectRobotinoComThread::update_nodata_timer()
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));
967 DirectRobotinoComThread::handle_nodata(
const boost::system::error_code &ec)