Switch to libev

CMakeLists.txt

@@ -22,6 +22,8 @@ find_package(catkin REQUIRED COMPONENTS
 find_package(Boost REQUIRED COMPONENTS system thread date_time filesystem)
 find_package(mavlink REQUIRED)
 
+include(libev.cmake)
+
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
@@ -141,6 +143,7 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
   ${mavlink_INCLUDE_DIRS}
+  ${libev_INCLUDE_DIR}
 )
 
 add_definitions(
@@ -172,6 +175,7 @@ add_dependencies(mavconn
 target_link_libraries(mavconn
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
+  ${libev_LIBRARY}
 )
 
 add_library(mavros_plugins

README.md

@@ -27,6 +27,7 @@ Catkin build system required (tested with ROS Hydro Medusa and Indigo Igloo).
 This package are dependent on ros-\*-mavlink build from [mavlink-gbp-release][7].
 Since 2014-06-19 it exists in hydro and indigo package index (so you can install via rosdep).
 
+Also since 2014-07-20 (pre 0.7.0) it depends on [libev-dev][10] system package.
 
 Programs
 --------
@@ -117,3 +118,4 @@ Links
 [7]: https://github.com/vooon/mavlink-gbp-release
 [8]: https://github.com/vooon/mavros/issues/35
 [9]: http://wiki.ros.org/mavros
+[10]: http://software.schmorp.de/pkg/libev

include/mavros/mavconn_interface.h

@@ -30,12 +30,14 @@
 
 #pragma once
 
-#include <boost/asio.hpp>
-#include <boost/bind.hpp>
 #include <boost/signals2.hpp>
-#include <boost/noncopyable.hpp>
+
+// cleanup {{
+#include <boost/bind.hpp>
+#include <boost/asio.hpp>
 #include <boost/thread/thread.hpp>
 #include <boost/thread/recursive_mutex.hpp>
+// }}
 
 #include <set>
 #include <memory>
@@ -45,10 +47,46 @@ namespace mavconn {
 namespace sig2 = boost::signals2;
 namespace asio = boost::asio;
 
+/**
+ * @brief Common exception for communication error
+ */
+class DeviceError : public std::exception {
+private:
+	std::string e_what_;
+
+public:
+	/**
+	 * @breif Construct error with description.
+	 */
+	explicit DeviceError(const char *module, const char *description) {
+		std::stringstream ss;
+		ss << "DeviceError:" << module << ": " << description;
+		e_what_ = ss.str();
+	}
+
+	/**
+	 * @brief Construct error from errno
+	 */
+	explicit DeviceError(const char *module, int errnum) {
+		std::ostringstream ss;
+		ss << "DeviceError:" << module << ":" << errnum << ": " << strerror(errnum);
+		e_what_ = ss.str();
+	}
+
+	DeviceError(const DeviceError& other) : e_what_(other.e_what_) {}
+	virtual ~DeviceError() throw() {}
+	virtual const char *what() const throw() {
+		return e_what_.c_str();
+	}
+};
+
 /**
  * @brief Generic mavlink interface
  */
-class MAVConnInterface : private boost::noncopyable {
+class MAVConnInterface {
+private:
+	MAVConnInterface(const MAVConnInterface&) = delete;
+
 public:
 	/**
 	 * @param[in] system_id     sysid for send_message
@@ -59,6 +97,8 @@ class MAVConnInterface : private boost::noncopyable {
 		delete_channel(channel);
 	};
 
+	virtual void close() = 0;
+
 	inline void send_message(const mavlink_message_t *message) {
 		send_message(message, sys_id, comp_id);
 	};
@@ -93,6 +133,18 @@ class MAVConnInterface : private boost::noncopyable {
 	inline uint8_t get_component_id() { return comp_id; };
 	inline void set_component_id(uint8_t compid) { comp_id = compid; };
 
+	/**
+	 * @brief Construct connection from URL
+	 * @param[in] url           resource locator
+	 * @param[in] system_id     optional system id
+	 * @param[in] component_id  optional component id
+	 *
+	 * @todo Implementation
+	 * @todo Documentation
+	 */
+	static boost::shared_ptr<MAVConnInterface> open_url(std::string url,
+			uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE);
+
 protected:
 	int channel;
 	uint8_t sys_id;
@@ -105,6 +157,8 @@ class MAVConnInterface : private boost::noncopyable {
 	static int new_channel();
 	static void delete_channel(int chan);
 
+	static void start_default_loop();
+
 private:
 	static std::set<int> allocated_channels;
 };

include/mavros/mavconn_msgbuffer.h

@@ -0,0 +1,87 @@
+/**
+ * @brief MAVConn message buffer class (internal)
+ * @file mavconn_msgbuffer.h
+ * @author Vladimir Ermakov <vooon341@gmail.com>
+ *
+ * @addtogroup mavconn
+ * @{
+ */
+/*
+ * Copyright 2014 Vladimir Ermakov.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#pragma once
+
+#include <mavros/mavconn_mavlink.h>
+#include <ros/assert.h>
+
+namespace mavconn {
+
+/**
+ * @brief Message buffer for internal use in libmavconn
+ */
+struct MsgBuffer {
+	//! Maximum buffer size with padding for CRC bytes (263 + 2 + align padding)
+	static constexpr ssize_t MAX_SIZE = MAVLINK_MAX_PACKET_LEN + 2 + 7;
+	uint8_t data[MAX_SIZE];
+	ssize_t len;
+	ssize_t pos;
+
+	MsgBuffer() :
+		pos(0),
+		len(0)
+	{ }
+
+	/**
+	 * @brief Buffer constructor from mavlink_message_t
+	 */
+	explicit MsgBuffer(const mavlink_message_t *msg) :
+		pos(0)
+	{
+		len = mavlink_msg_to_send_buffer(data, msg);
+		// paranoic check, it must be less than MAVLINK_MAX_PACKET_LEN
+		ROS_ASSERT(len < MAX_SIZE);
+	}
+
+	/**
+	 * @brief Buffer constructor for send_bytes()
+	 * @param[in] nbytes should be less than MAX_SIZE
+	 */
+	MsgBuffer(const uint8_t *bytes, ssize_t nbytes) :
+		pos(0),
+		len(nbytes)
+	{
+		ROS_ASSERT_MSG(0 < nbytes && nbytes < MAX_SIZE, "MsgBuffer overrun");
+		memcpy(data, bytes, nbytes);
+	}
+
+	virtual ~MsgBuffer() {
+		pos = 0;
+		len = 0;
+	}
+
+	uint8_t *dpos() {
+		return data + pos;
+	}
+
+	ssize_t nbytes() {
+		return len - pos;
+	}
+};
+
+}; // namespace mavconn
+

include/mavros/mavconn_serial.h

@@ -26,9 +26,10 @@
 
 #pragma once
 
+#include <list>
+#include <ev++.h>
 #include <mavros/mavconn_interface.h>
-#include <boost/asio/serial_port.hpp>
-#include <boost/shared_array.hpp>
+#include <mavros/mavconn_msgbuffer.h>
 
 namespace mavconn {
 
@@ -47,34 +48,25 @@ class MAVConnSerial : public MAVConnInterface {
 			std::string device = "/dev/ttyACM0", unsigned baudrate = 57600);
 	~MAVConnSerial();
 
+	void close();
+
 	using MAVConnInterface::send_message;
 	void send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid);
 	void send_bytes(const uint8_t *bytes, size_t length);
 
 	inline mavlink_status_t get_status() { return *mavlink_get_channel_status(channel); };
-	inline bool is_open() { return serial_dev.is_open(); };
+	inline bool is_open() { return fd != -1; };
 
 private:
-	asio::io_service io_service;
-	boost::thread io_thread;
-	asio::serial_port serial_dev;
+	ev::io io;
+	int fd;
 
-	static constexpr size_t RX_BUFSIZE = MAVLINK_MAX_PACKET_LEN;
-	uint8_t rx_buf[RX_BUFSIZE];
-	std::vector<uint8_t> tx_q;
-	static constexpr size_t TX_EXTENT = 256;	//!< extent size for tx buffer
-	static constexpr size_t TX_DELSIZE = 4096;	//!< buffer delete condition
-	boost::shared_array<uint8_t> tx_buf;
-	size_t tx_buf_size;				//!< size of current buffer()
-	size_t tx_buf_max_size;				//!< allocated buffer size
-	bool tx_in_process;				//!< tx status
+	std::list<MsgBuffer*> tx_q;
 	boost::recursive_mutex mutex;
 
-	void do_read(void);
-	void async_read_end(boost::system::error_code ec, size_t bytes_transfered);
-	void copy_and_async_write(void);
-	void do_write(void);
-	void async_write_end(boost::system::error_code ec);
+	void event_cb(ev::io &watcher, int revents);
+	void read_cb(ev::io &watcher);
+	void write_cb(ev::io &watcher);
 };
 
 }; // namespace mavconn

include/mavros/mavconn_udp.h

@@ -26,14 +26,20 @@
 
 #pragma once
 
+#include <list>
+#include <ev++.h>
 #include <mavros/mavconn_interface.h>
-#include <boost/asio/serial_port.hpp>
-#include <boost/shared_array.hpp>
+#include <mavros/mavconn_msgbuffer.h>
+
+#include <arpa/inet.h>
+#include <sys/socket.h>
 
 namespace mavconn {
 
 /**
  * @brief UDP interface
+ *
+ * @note IPv4 only
  */
 class MAVConnUDP : public MAVConnInterface {
 public:
@@ -48,39 +54,30 @@ class MAVConnUDP : public MAVConnInterface {
 			std::string listner_addr = "", unsigned short listner_port = 14550);
 	~MAVConnUDP();
 
+	void close();
+
 	using MAVConnInterface::send_message;
 	void send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid);
 	void send_bytes(const uint8_t *bytes, size_t length);
 
 	inline mavlink_status_t get_status() { return *mavlink_get_channel_status(channel); };
-	inline bool is_open() { return socket.is_open(); };
+	inline bool is_open() { return sockfd != -1; };
 
 private:
-	asio::io_service io_service;
-	std::unique_ptr<asio::io_service::work> io_work;
-	boost::thread io_thread;
-	asio::ip::udp::socket socket;
-	asio::ip::udp::endpoint server_endpoint;
-	asio::ip::udp::endpoint sender_endpoint;
-	asio::ip::udp::endpoint prev_sender_endpoint;
+	ev::io io;
+	int sockfd;
+
+	bool remote_exists;
+	sockaddr_in remote_addr;
+	sockaddr_in last_remote_addr;
+	sockaddr_in bind_addr;
 
-	static constexpr size_t RX_BUFSIZE = MAVLINK_MAX_PACKET_LEN;
-	uint8_t rx_buf[RX_BUFSIZE];
-	std::vector<uint8_t> tx_q;
-	static constexpr size_t TX_EXTENT = 256;	//!< extent size for tx buffer
-	static constexpr size_t TX_DELSIZE = 4096;	//!< buffer delete condition
-	boost::shared_array<uint8_t> tx_buf;
-	size_t tx_buf_size;				//!< size of current buffer()
-	size_t tx_buf_max_size;				//!< allocated buffer size
-	bool tx_in_process;				//!< tx status
+	std::list<MsgBuffer*> tx_q;
 	boost::recursive_mutex mutex;
-	bool sender_exists;
 
-	void do_read(void);
-	void async_read_end(boost::system::error_code ec, size_t bytes_transfered);
-	void copy_and_async_write(void);
-	void do_write(void);
-	void async_write_end(boost::system::error_code ec);
+	void event_cb(ev::io &watcher, int revents);
+	void read_cb(ev::io &watcher);
+	void write_cb(ev::io &watcher);
 };
 
 }; // namespace mavconn

libev.cmake

@@ -0,0 +1,12 @@
+# current libev don't provide any find-package or pkg-config files
+# so we just find library and header.
+
+find_library(libev_LIBRARY ev)
+find_path(libev_INCLUDE_DIR ev++.h
+	PATH_SUFFIXES include/ev include)
+find_package_handle_standard_args(libev DEFAULT_MSG libev_LIBRARY libev_INCLUDE_DIR)
+mark_as_advanced(libev_INCLUDE_DIR libev_LIBRARY)
+
+if (NOT libev_FOUND)
+	message(STATUS "The libev package not found. please install libev-dev")
+endif()

package.xml

@@ -32,6 +32,7 @@
   <build_depend>tf</build_depend>
   <build_depend>mavlink</build_depend>
   <build_depend>angles</build_depend>
+  <build_depend>libev-dev</build_depend>
   <run_depend>boost</run_depend>
   <run_depend>diagnostic_msgs</run_depend>
   <run_depend>diagnostic_updater</run_depend>
@@ -45,6 +46,7 @@
   <run_depend>tf</run_depend>
   <run_depend>mavlink</run_depend>
   <run_depend>angles</run_depend>
+  <run_depend>libev-dev</run_depend>
   <test_depend>gtest</test_depend>
 
   <export>

rosdep.yaml

@@ -0,0 +1,4 @@
+# temporary rosdep file before it merged to main list
+libev-dev:
+  debian: [libev-dev]
+  ubuntu: [libev-dev]