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> 31 #include <interfaces/BatteryInterface.h> 32 #include <interfaces/RobotinoSensorInterface.h> 33 #include <interfaces/IMUInterface.h> 38 #include <boost/bind.hpp> 39 #include <boost/lambda/bind.hpp> 40 #include <boost/lambda/lambda.hpp> 41 #include <boost/filesystem.hpp> 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_)
69 cfg_enable_gyro_ =
config->
get_bool(
"/hardware/robotino/gyro/enable");
70 cfg_sensor_update_cycle_time_ =
72 cfg_gripper_enabled_ =
config->
get_bool(
"/hardware/robotino/gripper/enable_gripper");
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");
83 if (find_controld3()) {
84 throw Exception(
"Found running controld3, stop using 'sudo initctl stop controld3'");
91 cfg_device_ = find_device_udev();
93 throw Exception(
"No udev support, must configure serial device");
97 deadline_.expires_at(boost::posix_time::pos_infin);
100 request_timer_.expires_from_now(boost::posix_time::milliseconds(-1));
101 drive_timer_.expires_at(boost::posix_time::pos_infin);
103 digital_outputs_ = 0;
107 checksum_errors_ = 0;
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);
145 update_nodata_timer();
159 checksum_errors_ = 0;
161 update_nodata_timer();
163 input_buffer_.consume(input_buffer_.size());
165 checksum_errors_ += 1;
166 if (cfg_log_checksum_errors_) {
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());
178 }
else if (checksum_errors_ >= cfg_checksum_error_critical_) {
180 input_buffer_.consume(input_buffer_.size());
189 input_buffer_.consume(input_buffer_.size());
204 logger->
log_info(
name(),
"Connection re-established after %u tries", open_tries_ + 1);
210 if (open_tries_ >= (1000 / cfg_sensor_update_cycle_time_)) {
222 bool new_data =
false;
224 DirectRobotinoComMessage::command_id_t msgid;
225 while ((msgid = m->next_command()) != DirectRobotinoComMessage::CMDID_NONE) {
228 if (msgid == DirectRobotinoComMessage::CMDID_ALL_MOTOR_READINGS) {
231 for (
int i = 0; i < 3; ++i)
data_.mot_velocity[i] = m->get_int16();
234 for (
int i = 0; i < 3; ++i)
data_.mot_position[i] = m->get_int32();
237 for (
int i = 0; i < 3; ++i)
data_.mot_current[i] = m->get_float();
240 }
else if (msgid == DirectRobotinoComMessage::CMDID_DISTANCE_SENSOR_READINGS) {
241 for (
int i = 0; i < 9; ++i)
data_.ir_voltages[i] = m->get_float();
244 }
else if (msgid == DirectRobotinoComMessage::CMDID_ALL_ANALOG_INPUTS) {
245 for (
int i = 0; i < 8; ++i)
data_.analog_in[i] = m->get_float();
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;
253 }
else if (msgid == DirectRobotinoComMessage::CMDID_BUMPER) {
254 data_.bumper = (m->get_uint8() != 0) ?
true :
false;
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();
263 }
else if (msgid == DirectRobotinoComMessage::CMDID_POWER_SOURCE_READINGS) {
264 float voltage = m->get_float();
265 float current = m->get_float();
267 data_.bat_voltage = voltage * 1000.;
268 data_.bat_current = current * 1000.;
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;
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();
282 id, mtime, error.c_str());
316 for (
int i = 0; i < 2; ++i) {
317 req.
add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_ACCEL_LIMITS);
332 if (digital_out < 1 || digital_out > 8) {
333 throw Exception(
"Invalid digital output, must be in range [1..8], got %u",
337 unsigned int digital_out_idx = digital_out - 1;
340 digital_outputs_ |= (1 << digital_out_idx);
342 digital_outputs_ &= ~(1 << digital_out_idx);
355 for (
int i = 0; i < 8; ++i)
data_.digital_out[i] = (digital_outputs_ & (1 << i)) ? true :
false;
363 return serial_.is_open();
371 a1 =
data_.mot_velocity[0];
372 a2 =
data_.mot_velocity[1];
373 a3 =
data_.mot_velocity[2];
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));
405 m.
add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
408 m.
add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
411 m.
add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
435 data_.bumper_estop_enabled = enabled;
444 DirectRobotinoComThread::find_device_udev()
446 std::string cfg_device =
"";
450 struct udev_enumerate *enumerate;
451 struct udev_list_entry *devices, *dev_list_entry;
452 struct udev_device *dev, *usb_device;
455 throw Exception(
"RobotinoDirect: Failed to initialize udev for " 459 enumerate = udev_enumerate_new(udev);
460 udev_enumerate_add_match_subsystem(enumerate,
"tty");
461 udev_enumerate_scan_devices(enumerate);
463 devices = udev_enumerate_get_list_entry(enumerate);
464 udev_list_entry_foreach(dev_list_entry, devices) {
467 path = udev_list_entry_get_name(dev_list_entry);
468 if (! path)
continue;
470 dev = udev_device_new_from_syspath(udev, path);
471 usb_device = udev_device_get_parent_with_subsystem_devtype(dev,
"usb",
473 if (! dev || ! usb_device)
continue;
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");
478 if (vendor_id ==
"1e29" && model_id ==
"040d") {
480 cfg_device = udev_device_get_property_value(dev,
"DEVNAME");
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");
496 model = model_from_db;
498 model = udev_device_get_property_value(dev,
"ID_MODEL");
501 vendor.c_str(), model.c_str(), cfg_device.c_str());
506 udev_enumerate_unref(enumerate);
509 if (cfg_device ==
"") {
510 throw Exception(
"No robotino device was found");
518 DirectRobotinoComThread::find_controld3()
521 boost::filesystem::path p(
"/proc");
526 if (boost::filesystem::exists(p) && boost::filesystem::is_directory(p)) {
528 directory_iterator di;
529 for (di = directory_iterator(p); di != directory_iterator(); ++di) {
530 directory_entry &d = *di;
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])) {
541 path pproc(d.path());
544 FILE *f = fopen(pproc.c_str(),
"r");
548 if (fscanf(f,
"%d (%m[a-z0-9])", &pid, &procname) == 2) {
550 if (strcmp(
"controld3", procname) == 0) {
561 logger->
log_warn(
name(),
"Cannot open /proc, cannot determine if controld3 is running");
565 }
catch (
const boost::filesystem::filesystem_error &ex) {
566 logger->
log_warn(
name(),
"Failure to determine if controld3 is running: %s", ex.what());
574 DirectRobotinoComThread::open_device(
bool wait_replies)
579 input_buffer_.consume(input_buffer_.size());
581 boost::mutex::scoped_lock lock(io_mutex_);
583 serial_.open(cfg_device_);
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));
589 }
catch (boost::system::system_error &e) {
590 throw Exception(
"RobotinoDirect failed I/O: %s", e.what());
595 req.
add_command(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
596 req.
add_command(DirectRobotinoComMessage::CMDID_GET_SW_VERSION);
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();
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();
616 id, mtime, error.c_str());
621 if (hw_version.empty() || sw_version.empty()) {
623 throw Exception(
"RobotinoDirect: no reply to version inquiry from robot");
626 hw_version.c_str(), sw_version.c_str());
640 DirectRobotinoComThread::close_device()
650 DirectRobotinoComThread::flush_device()
652 if (serial_.is_open()) {
654 boost::system::error_code ec = boost::asio::error::would_block;
655 size_t bytes_read = 0;
657 ec = boost::asio::error::would_block;
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));
666 do io_service_.run_one();
while (ec == boost::asio::error::would_block);
668 if (bytes_read > 0) {
672 }
while (bytes_read > 0);
673 deadline_.expires_from_now(boost::posix_time::pos_infin);
674 }
catch (boost::system::system_error &e) {
684 boost::mutex::scoped_lock lock(io_mutex_);
687 boost::system::error_code ec;
688 boost::asio::write(serial_, boost::asio::const_buffers_1(msg.
buffer()), ec);
692 throw Exception(
"Error while writing message (%s), closing connection",
693 ec.message().c_str());
698 std::shared_ptr<DirectRobotinoComMessage>
701 boost::mutex::scoped_lock lock(io_mutex_);
703 boost::system::error_code ec;
704 boost::asio::write(serial_, boost::asio::const_buffers_1(msg.
buffer()), ec);
707 ec.message().c_str());
710 throw Exception(
"RobotinoDirect: write failed (%s)", ec.message().c_str());
712 std::shared_ptr<DirectRobotinoComMessage> m = read_packet();
715 throw Exception(
"RobotinoDirect: serial device not opened");
721 class match_unescaped_length
724 explicit match_unescaped_length(
unsigned short length) : length_(length), l_(0) {}
726 template <
typename Iterator>
727 std::pair<Iterator, bool> operator()(
728 Iterator begin, Iterator end)
const 731 while (i != end && l_ < length_) {
732 if (*i++ != DirectRobotinoComMessage::MSG_DATA_ESCAPE) {
736 return std::make_pair(i, (l_ == length_));
740 unsigned short length_;
741 mutable unsigned short l_;
746 template <>
struct is_match_condition<match_unescaped_length>
747 :
public boost::true_type {};
753 DirectRobotinoComThread::read_packet()
755 boost::system::error_code ec = boost::asio::error::would_block;
756 size_t bytes_read = 0;
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));
762 do io_service_.run_one();
while (ec == boost::asio::error::would_block);
765 if (ec.value() == boost::system::errc::operation_canceled) {
766 throw Exception(
"Timeout (1) on initial synchronization");
768 throw Exception(
"Error (1) on initial synchronization: %s", ec.message().c_str());
776 input_buffer_.consume(bytes_read - 1);
779 deadline_.expires_from_now(boost::posix_time::milliseconds(cfg_read_timeout_));
782 ec = boost::asio::error::would_block;
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));
789 do io_service_.run_one();
while (ec == boost::asio::error::would_block);
792 if (ec.value() == boost::system::errc::operation_canceled) {
793 throw Exception(
"Timeout (2) on initial synchronization");
795 throw Exception(
"Error (2) on initial synchronization: %s", ec.message().c_str());
799 const unsigned char *length_escaped =
800 boost::asio::buffer_cast<
const unsigned char*>(input_buffer_.data());
802 unsigned char length_unescaped[2];
805 unsigned short length =
809 ec = boost::asio::error::would_block;
811 boost::asio::async_read_until(serial_, input_buffer_,
812 match_unescaped_length(length + 2),
813 (boost::lambda::var(ec) = boost::lambda::_1,
814 boost::lambda::var(bytes_read) = boost::lambda::_2));
816 do io_service_.run_one();
while (ec == boost::asio::error::would_block);
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());
823 throw Exception(
"Error (3) on initial synchronization: %s", ec.message().c_str());
827 deadline_.expires_at(boost::posix_time::pos_infin);
830 (boost::asio::buffer_cast<
const unsigned char*> (input_buffer_.data()), input_buffer_.size());
832 input_buffer_.consume(m->escaped_data_size());
844 DirectRobotinoComThread::check_deadline()
846 if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) {
848 deadline_.expires_at(boost::posix_time::pos_infin);
851 deadline_.async_wait(boost::lambda::bind(&DirectRobotinoComThread::check_deadline,
this));
858 DirectRobotinoComThread::request_data()
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));
870 DirectRobotinoComThread::handle_request_data(
const boost::system::error_code &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);
915 DirectRobotinoComThread::drive()
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));
926 DirectRobotinoComThread::handle_drive(
const boost::system::error_code &ec)
935 DirectRobotinoComThread::update_nodata_timer()
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));
944 DirectRobotinoComThread::handle_nodata(
const boost::system::error_code &ec)
SensorData data_
Data struct that must be updated whenever new data is available.
virtual void finalize()
Finalize the thread.
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
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.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void loop()
Code to execute in the thread.
bool update_velocities()
Update velocity values.
A class for handling time.
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().
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.
fawkes::Mutex * data_mutex_
Mutex to protect data_.
virtual void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
Robotino communication message.
virtual void once()
Execute an action exactly once.
Base class for exceptions in Fawkes.
virtual bool is_gripper_open()
Check if gripper is open.
Virtual base class for thread that communicates with a Robotino.
const char * name() const
Get name of thread.
virtual void init()
Initialize the thread.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
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.
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.