[Fawkes Git] branch/timn/robotino-direct: 2 revs updated. (0.5.0-3092-g1bcb9f0)

Tim Niemueller niemueller at kbsg.rwth-aachen.de
Mon Apr 11 22:05:03 CEST 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, timn/robotino-direct has been updated
        to  1bcb9f08a375c4b148c5a474d55ebb36ae6407e2 (commit)
       via  3968030a9cd0427c4ef26377dec10ad337d871f4 (commit)
      from  ce0a8fcdb3b7ba739e619287bb23064724c179eb (commit)

http://git.fawkesrobotics.org/fawkes.git/timn/robotino-direct

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- *Log* ---------------------------------------------------------------
commit 3968030a9cd0427c4ef26377dec10ad337d871f4
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Mon Apr 11 22:00:10 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Mon Apr 11 22:00:10 2016 +0200

    robotino: allow deserializing multiple messages from buffer
    
    Involves parsing not all from the buffer but the expected length of the
    message according to its meta data.

http://git.fawkesrobotics.org/fawkes.git/commit/3968030
http://trac.fawkesrobotics.org/changeset/3968030

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 1bcb9f08a375c4b148c5a474d55ebb36ae6407e2
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Mon Apr 11 22:02:13 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Mon Apr 11 22:02:13 2016 +0200

    robotino: decouple sending and receiving in direct comm
    
    Instead of sending messages and immediately waiting for the reply,
    decouple sending from receiving. This results in a more robust
    communication with the base and about the same packet loss as controld3.

http://git.fawkesrobotics.org/fawkes.git/commit/1bcb9f0
http://trac.fawkesrobotics.org/changeset/1bcb9f0

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------
 src/plugins/robotino/com_thread.h           |    4 +
 src/plugins/robotino/direct_com_message.cpp |   87 +++++++++---
 src/plugins/robotino/direct_com_message.h   |    8 +-
 src/plugins/robotino/direct_com_thread.cpp  |  218 +++++++++++++++++++-------
 src/plugins/robotino/direct_com_thread.h    |   19 ++-
 5 files changed, 249 insertions(+), 87 deletions(-)


- *Diffs* -------------------------------------------------------------

- *commit* 3968030a9cd0427c4ef26377dec10ad337d871f4 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Mon Apr 11 22:00:10 2016 +0200
Subject: robotino: allow deserializing multiple messages from buffer

 src/plugins/robotino/direct_com_message.cpp |   87 +++++++++++++++++++++------
 src/plugins/robotino/direct_com_message.h   |    8 ++-
 2 files changed, 74 insertions(+), 21 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/direct_com_message.cpp b/src/plugins/robotino/direct_com_message.cpp
index 57b4ead..07a0abe 100644
--- a/src/plugins/robotino/direct_com_message.cpp
+++ b/src/plugins/robotino/direct_com_message.cpp
@@ -98,9 +98,12 @@ DirectRobotinoComMessage::DirectRobotinoComMessage(const unsigned char *msg, siz
 	escaped_data_ = (unsigned char *)malloc(msg_size);
 	memcpy(escaped_data_, msg, msg_size);
 	escaped_data_size_ = msg_size;
-	unescape_data();
+	size_t escaped_consumed = unescape_data();
 
-	payload_size_ = parse_uint16(&msg[1]);
+	if (escaped_consumed < msg_size) {
+		escaped_data_ = (unsigned char *)realloc(escaped_data_, escaped_consumed);
+		escaped_data_size_ = escaped_consumed;
+	}
 
 	check_checksum();
 }
@@ -625,6 +628,39 @@ DirectRobotinoComMessage::skip_float()
 	cur_data_ += 4;
 }
 
+
+/** Size of escaped buffer.
+ * In particular, after calling the parsing ctor this denotes how many
+ * bytes of the input buffer have been consumed.
+ * On message creation, only provides meaningful values after calling
+ * pack() or buffer().
+ * @return size of escaped buffer
+ */
+size_t
+DirectRobotinoComMessage::escaped_data_size()
+{
+	return escaped_data_size_;
+}
+
+
+/** Get payload size.
+ * @return payload size
+ */
+size_t
+DirectRobotinoComMessage::payload_size()
+{
+	return payload_size_;
+}
+
+/** Get internal data buffer size.
+ * @return data buffer size
+ */
+size_t
+DirectRobotinoComMessage::data_size()
+{
+	return data_size_;
+}
+
 /** Perform message escaping. */
 void
 DirectRobotinoComMessage::escape()
@@ -656,18 +692,22 @@ DirectRobotinoComMessage::escape()
 	}
 }
 
-/** Unescape given buffer.
+/** Unescape a number of unescaped bytes.
  * @param unescaped buffer to contain the unescaped data on return,
- * must be at least of \p escaped_size number of bytes
+ * must be at least of \p unescaped_size number of bytes
+ * @param unescaped_size expected number of bytes to unescape
+ * from input buffer
  * @param escaped escaped buffer to process
  * @param escaped_size size of escaped_buffer
- * @return number of bytes in \p unescaped
+ * @return number of bytes consumed from escaped buffer
+ * @throw Exception if not enough bytes could be unescaped
  */
 size_t
-DirectRobotinoComMessage::unescape(unsigned char *unescaped,
+DirectRobotinoComMessage::unescape(unsigned char *unescaped, size_t unescaped_size,
                                    const unsigned char *escaped, size_t escaped_size)
 {
-	size_t unescaped_size = 0;
+	if (unescaped_size == 0)  return 0;
+
 	unsigned int j = 0;
 	for (unsigned int i = 0; i < escaped_size; ++i) {
 		if (escaped[i] == MSG_DATA_ESCAPE) {
@@ -679,32 +719,41 @@ DirectRobotinoComMessage::unescape(unsigned char *unescaped,
 		} else {
 			unescaped[j++] = escaped[i];
 		}
-		unescaped_size += 1;
+		if (j == unescaped_size)  return i+1;
 	}
 
-	return unescaped_size;
+	throw Exception("Not enough escaped bytes for unescaping");
 }
 
-/** Unescape all data. */
-void
+/** Unescape all data.
+ * @return number of bytes consumed from escaped buffer
+ */
+size_t
 DirectRobotinoComMessage::unescape_data()
 {
 	if (! escaped_data_ || escaped_data_size_ == 0) {
 		throw Exception("No escaped data to unescape");
 	}
 
-	if (data_size_ < escaped_data_size_) {
-		data_ = (unsigned char *)realloc(data_, escaped_data_size_);
-		data_size_ = escaped_data_size_;
+	if (data_size_ < 3) {
+		data_ = (unsigned char *)realloc(data_, 3);
+		data_[0] = MSG_HEAD;
 	}
+	// +1: HEAD
+	size_t consumed_bytes = unescape(&data_[1], 2, &escaped_data_[1], escaped_data_size_-1) + 1;
+	size_t unescaped_size = parse_uint16(&data_[1]) + 2; // +2: checksum
 
-	size_t unescaped_size =
-		unescape(&data_[1], &escaped_data_[1], escaped_data_size_ - 1);
+	if (data_size_ < unescaped_size + 3) {
+		data_ = (unsigned char *)realloc(data_, unescaped_size + 3); // +3: HEAD, LENGTH
+		data_size_ = unescaped_size + 3;
+	}
+	payload_size_ = unescaped_size - 2; // -2: no checksum
 
-	// +1: HEAD
-	unescaped_size += 1;
+	consumed_bytes +=
+		unescape(&data_[3], unescaped_size,
+		         &escaped_data_[consumed_bytes], escaped_data_size_ - consumed_bytes);
 
-	payload_size_ = unescaped_size - MSG_METADATA_SIZE;
+	return consumed_bytes;
 }
 
 
diff --git a/src/plugins/robotino/direct_com_message.h b/src/plugins/robotino/direct_com_message.h
index 29fa72f..ba3556e 100644
--- a/src/plugins/robotino/direct_com_message.h
+++ b/src/plugins/robotino/direct_com_message.h
@@ -184,11 +184,15 @@ class DirectRobotinoComMessage
 
 	std::string to_string(bool escaped = false);
 
-	static size_t unescape(unsigned char *unescaped,
+	static size_t unescape(unsigned char *unescaped, size_t unescaped_size,
 	                       const unsigned char *escaped, size_t escaped_size);
 
 	static uint16_t parse_uint16(const unsigned char *buf);
 
+	size_t escaped_data_size();
+	size_t payload_size();
+	size_t data_size();
+	
  private:
 	void ctor();
 	void assert_mode(mode_t mode) const;
@@ -197,7 +201,7 @@ class DirectRobotinoComMessage
 
 	void inc_payload_by(uint16_t count);
 	void escape();
-	void unescape_data();
+	size_t unescape_data();
 	void check_checksum() const;
   
  private:

- *commit* 1bcb9f08a375c4b148c5a474d55ebb36ae6407e2 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Mon Apr 11 22:02:13 2016 +0200
Subject: robotino: decouple sending and receiving in direct comm

 src/plugins/robotino/com_thread.h          |    4 +
 src/plugins/robotino/direct_com_thread.cpp |  218 ++++++++++++++++++++--------
 src/plugins/robotino/direct_com_thread.h   |   19 ++-
 3 files changed, 175 insertions(+), 66 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robotino/com_thread.h b/src/plugins/robotino/com_thread.h
index 74fef51..168d39a 100644
--- a/src/plugins/robotino/com_thread.h
+++ b/src/plugins/robotino/com_thread.h
@@ -68,6 +68,10 @@ class RobotinoComThread
 		float        imu_angular_velocity[3];
 		double       imu_angular_velocity_covariance[9];
 
+		float        odo_x;
+		float        odo_y;
+		float        odo_phi;
+
 		float        ir_voltages[NUM_IR_SENSORS];
 		
 		fawkes::Time time;
diff --git a/src/plugins/robotino/direct_com_thread.cpp b/src/plugins/robotino/direct_com_thread.cpp
index cf67ef9..4bfe0a2 100644
--- a/src/plugins/robotino/direct_com_thread.cpp
+++ b/src/plugins/robotino/direct_com_thread.cpp
@@ -1,9 +1,9 @@
 
 /***************************************************************************
- *  com_thread.cpp - Robotino com thread
+ *  direct_com_thread.cpp - Robotino com thread for direct communication
  *
- *  Created: Thu Sep 11 13:18:00 2014
- *  Copyright  2011-2014  Tim Niemueller [www.niemueller.de]
+ *  Created: Mon Apr 04 11:48:36 2016
+ *  Copyright  2011-2016  Tim Niemueller [www.niemueller.de]
  ****************************************************************************/
 
 /*  This program is free software; you can redistribute it and/or modify
@@ -35,6 +35,7 @@
 #include <unistd.h>
 
 #include <libudev.h>
+#include <boost/bind.hpp>
 #include <boost/lambda/bind.hpp>
 #include <boost/lambda/lambda.hpp>
 
@@ -48,8 +49,10 @@ using namespace fawkes;
 /** Constructor. */
 DirectRobotinoComThread::DirectRobotinoComThread()
 	: RobotinoComThread("DirectRobotinoComThread"),
-	  serial_(io_service_), io_service_work_(io_service_), deadline_(io_service_)
+	  serial_(io_service_), io_service_work_(io_service_), deadline_(io_service_),
+	  request_timer_(io_service_), nodata_timer_(io_service_)
 {
+	set_prepfin_conc_loop(true);
 }
 
 
@@ -62,15 +65,11 @@ DirectRobotinoComThread::~DirectRobotinoComThread()
 void
 DirectRobotinoComThread::init()
 {
-	cfg_hostname_ = config->get_string("/hardware/robotino/hostname");
 	cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
 	cfg_sensor_update_cycle_time_ =
 		config->get_uint("/hardware/robotino/sensor_update_cycle_time");
 	cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
 
-	last_seqnum_ = 0;
-	time_wait_   = new TimeWait(clock, cfg_sensor_update_cycle_time_ * 1000);
-
 	// -------------------------------------------------------------------------- //
 
 	try {
@@ -92,57 +91,63 @@ DirectRobotinoComThread::init()
 }
 
 
+bool
+DirectRobotinoComThread::prepare_finalize_user()
+{
+	//logger->log_info(name(), "Prepare Finalize");
+	request_timer_.cancel();
+	nodata_timer_.cancel();
+	request_timer_.expires_at(boost::posix_time::pos_infin);
+	nodata_timer_.expires_at(boost::posix_time::pos_infin);
+	deadline_.expires_at(boost::posix_time::pos_infin);
+
+	serial_.cancel();
+	
+	return true;
+}
+
 void
 DirectRobotinoComThread::finalize()
 {
-	delete time_wait_;
+	close_device();
 }
 
 void
 DirectRobotinoComThread::once()
 {
 	reset_odometry();
+	request_data();
+	update_nodata_timer();
 }
 
 void
 DirectRobotinoComThread::loop()
 {
-	time_wait_->mark_start();
+	if (finalize_prepared) {
+		usleep(1000);
+		return;
+	}
 
 	if (opened_) {
-		DirectRobotinoComMessage req;
-		req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_MOTOR_READINGS);
-		req.add_command(DirectRobotinoComMessage::CMDID_GET_DISTANCE_SENSOR_READINGS);
-		req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_ANALOG_INPUTS);
-		req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_DIGITAL_INPUTS);
-		req.add_command(DirectRobotinoComMessage::CMDID_GET_BUMPER);
-		req.add_command(DirectRobotinoComMessage::CMDID_GET_GYRO_Z_ANGLE);
-		// This command is documented in the wiki but does not exist in the
-		// enum and is not handled in the firmware. Instead, the information
-		// is sent with every reply, so we also get it here automatically.
-		// This has been checked in Robotino3 firmware 1.1.1
-		// req.add_command(DirectRobotinoComMessage::CMDID_GET_POWER_SOURCE_READINGS);
-
-		//req.pack();
-		//logger->log_debug(name(), "Req1:\n%s", req.to_string().c_str());
-
 		try {
-			DirectRobotinoComMessage::pointer m = send_and_recv(req);
-	
-			MutexLocker lock(data_mutex_);
-			new_data_ = true;
-			data_.seq += 1;
-
+			DirectRobotinoComMessage::pointer m = read_packet();
 			process_message(m);
+			update_nodata_timer();
 		} catch (Exception &e) {
-			logger->log_warn(name(), "Transmission error, re-connecting, exception follows");
-			logger->log_warn(name(), e);
-			opened_ = false;
-			open_tries_ = 0;
-			close_device();
+			if (! finalize_prepared) {
+				logger->log_warn(name(), "Transmission error, sending ping");
+				logger->log_warn(name(), e);
+				input_buffer_.consume(input_buffer_.size());
+				DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
+				send_message(req);
+				//} else {
+				//logger->log_warn(name(), "Transmission error, but finalize prepared");
+				//logger->log_warn(name(), e);
+			}
 		}
 	} else {
 		try {
+			logger->log_info(name(), "Re-opening device");
 			open_device();
 			opened_ = true;
 			logger->log_info(name(), "Connection re-established after %u tries", open_tries_ + 1);
@@ -154,14 +159,14 @@ DirectRobotinoComThread::loop()
 			}
 		}
 	}
-
-	time_wait_->wait();
 }
 
 
 void
 DirectRobotinoComThread::process_message(DirectRobotinoComMessage::pointer m)
 {
+	bool new_data = false;
+
 	DirectRobotinoComMessage::command_id_t msgid;
 	while ((msgid = m->next_command()) != DirectRobotinoComMessage::CMDID_NONE) {
 		//logger->log_info(name(), "Command length: %u", m->command_length());
@@ -176,19 +181,30 @@ DirectRobotinoComThread::process_message(DirectRobotinoComMessage::pointer m)
 			m->skip_int32();
 
 			for (int i = 0; i < 3; ++i)  data_.mot_current[i] = m->get_float();
+			new_data = true;
 
 		} else if (msgid == DirectRobotinoComMessage::CMDID_DISTANCE_SENSOR_READINGS) {
 			for (int i = 0; i < 9; ++i)  data_.ir_voltages[i] = m->get_float();
+			new_data = true;
 
 		} else if (msgid == DirectRobotinoComMessage::CMDID_ALL_ANALOG_INPUTS) {
 			for (int i = 0; i < 8; ++i)  data_.analog_in[i] = m->get_float();
+			new_data = true;
 
 		} else if (msgid == DirectRobotinoComMessage::CMDID_ALL_DIGITAL_INPUTS) {
 			uint8_t value = m->get_uint8();
 			for (int i = 0; i < 8; ++i)  data_.digital_in[i] = (value & (1 << i)) ? true : false;
+			new_data = true;
 
 		} else if (msgid == DirectRobotinoComMessage::CMDID_BUMPER) {
 			data_.bumper = (m->get_uint8() != 0) ? true : false;
+			new_data = true;
+
+		} else if (msgid == DirectRobotinoComMessage::CMDID_ODOMETRY) {
+			data_.odo_x   = m->get_float();
+			data_.odo_y   = m->get_float();
+			data_.odo_phi = m->get_float();
+			new_data = true;
 
 		} else if (msgid == DirectRobotinoComMessage::CMDID_POWER_SOURCE_READINGS) {
 			float voltage = m->get_float();
@@ -202,6 +218,8 @@ DirectRobotinoComThread::process_message(DirectRobotinoComMessage::pointer m)
 			soc = std::min(1.f, std::max(0.f, soc));
 			data_.bat_absolute_soc = soc;
 
+			new_data = true;
+
 		} else if (msgid == DirectRobotinoComMessage::CMDID_CHARGER_ERROR) {
 			uint8_t id = m->get_uint8();
 			uint32_t mtime = m->get_uint32();
@@ -210,6 +228,13 @@ DirectRobotinoComThread::process_message(DirectRobotinoComMessage::pointer m)
 			                 id, mtime, error.c_str());
 		}
 	}
+
+	if (new_data) {
+		MutexLocker lock(data_mutex_);
+		new_data_ = true;
+		data_.seq += 1;
+		data_.time.stamp();
+	}
 }
 
 
@@ -247,11 +272,10 @@ DirectRobotinoComThread::get_act_velocity(float &a1, float &a2, float &a3, unsig
 void
 DirectRobotinoComThread::get_odometry(double &x, double &y, double &phi)
 {
-	DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_GET_ODOMETRY);
-	DirectRobotinoComMessage::pointer m = send_and_recv(req);
-	x   = m->get_float();
-	y   = m->get_float();
-	phi = m->get_float();
+	MutexLocker lock(data_mutex_);
+	x   = data_.odo_x;
+	y   = data_.odo_y;
+	phi = data_.odo_phi;
 }
 
 bool
@@ -288,6 +312,9 @@ DirectRobotinoComThread::set_bumper_estop_enabled(bool enabled)
 	DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_EMERGENCY_BUMPER);
 	m.add_uint8(enabled ? 1 : 0);
 	send_message(m);
+
+	MutexLocker lock(data_mutex_);
+	data_.bumper_estop_enabled = enabled;
 }
 
 
@@ -367,6 +394,8 @@ DirectRobotinoComThread::find_device_udev()
 void
 DirectRobotinoComThread::open_device()
 {
+	if (finalize_prepared)  return;
+
 	try {
 		input_buffer_.consume(input_buffer_.size());
 
@@ -465,7 +494,10 @@ void
 DirectRobotinoComThread::send_message(DirectRobotinoComMessage &msg)
 {
 	boost::mutex::scoped_lock lock(io_mutex_);
-	boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()));
+	if (opened_) {
+		//logger->log_warn(name(), "Sending");
+		boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()));
+	}
 }
 
 /*
@@ -481,13 +513,7 @@ DirectRobotinoComThread::send_and_recv(DirectRobotinoComMessage &msg)
 {
 	boost::mutex::scoped_lock lock(io_mutex_);
 	boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()));
-	read_packet();
-
-	std::shared_ptr<DirectRobotinoComMessage> m =
-		std::make_shared<DirectRobotinoComMessage>(boost::asio::buffer_cast<const unsigned char*>(input_buffer_.data()),
-		                                           input_buffer_.size());
-
-	input_buffer_.consume(input_buffer_.size());
+	std::shared_ptr<DirectRobotinoComMessage> m = read_packet();
 	return m;
 }
 
@@ -524,13 +550,12 @@ namespace boost {
 }
 /// @endcond
 
-void
+DirectRobotinoComMessage::pointer
 DirectRobotinoComThread::read_packet()
 {
 	boost::system::error_code ec = boost::asio::error::would_block;
 	size_t bytes_read = 0;
 
-	deadline_.expires_from_now(boost::posix_time::milliseconds(200));
 	boost::asio::async_read_until(serial_, input_buffer_, DirectRobotinoComMessage::MSG_HEAD,
 	                              (boost::lambda::var(ec) = boost::lambda::_1,
 	                               boost::lambda::var(bytes_read) = boost::lambda::_2));
@@ -550,6 +575,9 @@ DirectRobotinoComThread::read_packet()
 		logger->log_warn(name(), "Read junk off line");
 	}
 	input_buffer_.consume(bytes_read - 1);
+	
+	// start timeout for remaining packet
+	deadline_.expires_from_now(boost::posix_time::milliseconds(200));
 
 	// read packet length
 	ec = boost::asio::error::would_block;
@@ -572,11 +600,9 @@ DirectRobotinoComThread::read_packet()
 	const unsigned char *length_escaped =
 		boost::asio::buffer_cast<const unsigned char*>(input_buffer_.data());
 
-	unsigned char *length_unescaped = (unsigned char *)malloc(bytes_read);
-	size_t length_unescaped_size __attribute__((unused)) =
-		DirectRobotinoComMessage::unescape(length_unescaped, &length_escaped[1], bytes_read);
+	unsigned char length_unescaped[2];
+	DirectRobotinoComMessage::unescape(length_unescaped, 2, &length_escaped[1], bytes_read);
 
-	// if (length_unescaped_size != 2) fail
 	unsigned short length =
 		DirectRobotinoComMessage::parse_uint16(length_unescaped);
 
@@ -584,7 +610,7 @@ DirectRobotinoComThread::read_packet()
 	ec = boost::asio::error::would_block;
 	bytes_read = 0;
 	boost::asio::async_read_until(serial_, input_buffer_,
-	                              match_unescaped_length(length),
+	                              match_unescaped_length(length + 2), // +2: checksum
 	                              (boost::lambda::var(ec) = boost::lambda::_1,
 	                               boost::lambda::var(bytes_read) = boost::lambda::_2));
 
@@ -600,6 +626,13 @@ DirectRobotinoComThread::read_packet()
 	}
 
 	deadline_.expires_at(boost::posix_time::pos_infin);
+
+	DirectRobotinoComMessage::pointer m = std::make_shared<DirectRobotinoComMessage>
+		(boost::asio::buffer_cast<const unsigned char*> (input_buffer_.data()), input_buffer_.size());
+
+	input_buffer_.consume(m->escaped_data_size());
+
+	return m;
 }
 
 
@@ -618,3 +651,70 @@ DirectRobotinoComThread::check_deadline()
 
 	deadline_.async_wait(boost::lambda::bind(&DirectRobotinoComThread::check_deadline, this));
 }
+
+
+/** Request new data.
+ */
+void
+DirectRobotinoComThread::request_data()
+{
+	if (finalize_prepared)  return;
+
+	request_timer_.expires_from_now(boost::posix_time::milliseconds(cfg_sensor_update_cycle_time_));
+	request_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_request_data, this,
+	                                      boost::asio::placeholders::error));
+}
+
+void
+DirectRobotinoComThread::handle_request_data(const boost::system::error_code &ec)
+{
+	if (! ec) {
+		DirectRobotinoComMessage req;
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_MOTOR_READINGS);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_DISTANCE_SENSOR_READINGS);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_ANALOG_INPUTS);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_ALL_DIGITAL_INPUTS);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_BUMPER);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_ODOMETRY);
+		req.add_command(DirectRobotinoComMessage::CMDID_GET_GYRO_Z_ANGLE);
+		// This command is documented in the wiki but does not exist in the
+		// enum and is not handled in the firmware. Instead, the information
+		// is sent with every reply, so we also get it here automatically.
+		// This has been checked in Robotino3 firmware 1.1.1
+		// req.add_command(DirectRobotinoComMessage::CMDID_GET_POWER_SOURCE_READINGS);
+
+		//req.pack();
+		//logger->log_debug(name(), "Req1:\n%s", req.to_string().c_str());
+
+		try {
+			send_message(req);
+		} catch (Exception &e) {
+			logger->log_warn(name(), "Sending message failed, excpeption follows");
+			logger->log_warn(name(), e);
+		}
+
+		request_data();
+	}
+}
+
+
+void
+DirectRobotinoComThread::update_nodata_timer()
+{
+	nodata_timer_.cancel();
+	nodata_timer_.expires_from_now(boost::posix_time::milliseconds(1000));
+	nodata_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_nodata, this,
+	                                     boost::asio::placeholders::error));
+}
+
+void
+DirectRobotinoComThread::handle_nodata(const boost::system::error_code &ec)
+{
+	// ec may be set if the timer is cancelled, i.e., updated
+	if (! ec) {
+		logger->log_error(name(), "No data received for too long, re-establishing connection");
+		close_device();
+		opened_ = false;
+		open_tries_ = 0;
+	}
+}
diff --git a/src/plugins/robotino/direct_com_thread.h b/src/plugins/robotino/direct_com_thread.h
index a840a30..a20684d 100644
--- a/src/plugins/robotino/direct_com_thread.h
+++ b/src/plugins/robotino/direct_com_thread.h
@@ -60,6 +60,8 @@ class DirectRobotinoComThread
 	virtual void init();
 	virtual void once();
 	virtual void loop();
+	bool    prepare_finalize_user();
+
 	virtual void finalize();
 
 	virtual bool is_connected();
@@ -81,16 +83,19 @@ class DirectRobotinoComThread
 	void close_device();
 	void flush_device();
 	void check_deadline();
-
-	void read_packet();
+	void request_data();
+	void handle_request_data(const boost::system::error_code &ec);
+	void handle_nodata(const boost::system::error_code &ec);
+	void update_nodata_timer();
+	
+	DirectRobotinoComMessage::pointer read_packet();
 	void send_message(DirectRobotinoComMessage &msg);
-	std::shared_ptr<DirectRobotinoComMessage>
+	DirectRobotinoComMessage::pointer
 		send_and_recv(DirectRobotinoComMessage &msg);
 	void process_message(DirectRobotinoComMessage::pointer m);
 
  private:
 	std::string     cfg_device_;
-	std::string     cfg_hostname_;
 	bool            cfg_enable_gyro_;
 	unsigned int    cfg_sensor_update_cycle_time_;
 	bool            cfg_gripper_enabled_;
@@ -98,15 +103,15 @@ class DirectRobotinoComThread
 	bool opened_;
 	unsigned int open_tries_;
 
-	fawkes::TimeWait *time_wait_;
-	unsigned int      last_seqnum_;
-
 	boost::asio::io_service       io_service_;
 	boost::asio::serial_port      serial_;
 	boost::asio::io_service::work io_service_work_;
 	boost::asio::deadline_timer   deadline_;
 	boost::asio::streambuf        input_buffer_;
 	boost::mutex                  io_mutex_;
+
+	boost::asio::deadline_timer   request_timer_;
+	boost::asio::deadline_timer   nodata_timer_;
 	
 };
 




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list