Merge pull request #139 from DataspeedInc/ros1/udp

ROS1: Add UDP support

README.md

@@ -1,5 +1,5 @@
 # ublox
-The `ublox` package provides support for [u-blox](http://www.u-blox.com) GPS receivers. Only the _serial_ configuration of the driver is documented here, but TCP communication is also supported by the driver (untested).
+The `ublox` package provides support for [u-blox](http://www.u-blox.com) GPS receivers. Only the _serial_ configuration of the driver is documented here, but TCP/UDP communication is also supported by the driver (untested).
 
 The driver was originally written by Johannes Meyer. Changes made later are detailed in the version history below.
 

ublox_gps/include/ublox_gps/async_worker.h

@@ -199,6 +199,28 @@ void AsyncWorker<StreamT>::doWrite() {
   out_.clear();
   write_condition_.notify_all();
 }
+template <>
+inline void AsyncWorker<boost::asio::ip::udp::socket>::doWrite() {
+  ScopedLock lock(write_mutex_);
+  // Do nothing if out buffer is empty
+  if (out_.size() == 0) {
+    return;
+  }
+  // Write all the data in the out buffer
+  stream_->send(boost::asio::buffer(out_.data(), out_.size()));
+
+  if (debug >= 2) {
+    // Print the data that was sent
+    std::ostringstream oss;
+    for (std::vector<unsigned char>::iterator it = out_.begin();
+         it != out_.end(); ++it)
+      oss << boost::format("%02x") % static_cast<unsigned int>(*it) << " ";
+    ROS_DEBUG("U-Blox sent %li bytes: \n%s", out_.size(), oss.str().c_str());
+  }
+  // Clear the buffer & unlock
+  out_.clear();
+  write_condition_.notify_all();
+}
 
 template <typename StreamT>
 void AsyncWorker<StreamT>::doRead() {
@@ -210,6 +232,16 @@ void AsyncWorker<StreamT>::doRead() {
                               boost::asio::placeholders::error,
                               boost::asio::placeholders::bytes_transferred));
 }
+template <>
+inline void AsyncWorker<boost::asio::ip::udp::socket>::doRead() {
+  ScopedLock lock(read_mutex_);
+  stream_->async_receive(
+      boost::asio::buffer(in_.data() + in_buffer_size_,
+                          in_.size() - in_buffer_size_),
+                          boost::bind(&AsyncWorker<boost::asio::ip::udp::socket>::readEnd, this,
+                              boost::asio::placeholders::error,
+                              boost::asio::placeholders::bytes_transferred));
+}
 
 template <typename StreamT>
 void AsyncWorker<StreamT>::readEnd(const boost::system::error_code& error,

ublox_gps/include/ublox_gps/gps.h

@@ -36,6 +36,7 @@
 #include <stdexcept>
 // Boost
 #include <boost/asio/ip/tcp.hpp>
+#include <boost/asio/ip/udp.hpp>
 #include <boost/asio/serial_port.hpp>
 #include <boost/asio/io_service.hpp>
 #include <boost/atomic.hpp>
@@ -96,6 +97,13 @@ class Gps {
    */
   void initializeTcp(std::string host, std::string port);
 
+  /**
+   * @brief Initialize UDP I/O.
+   * @param host the UDP host
+   * @param port the UDP port
+   */
+  void initializeUdp(std::string host, std::string port);
+
   /**
    * @brief Initialize the Serial I/O port.
    * @param port the device port address

ublox_gps/include/ublox_gps/node.h

@@ -489,7 +489,9 @@ typedef boost::shared_ptr<ComponentInterface> ComponentPtr;
  */
 class UbloxNode : public virtual ComponentInterface {
  public:
-  //! how often (in seconds) to call poll messages
+  //! How often (in seconds) to send keep-alive message
+  constexpr static double kKeepAlivePeriod = 10.0;
+  //! How often (in seconds) to call poll messages
   constexpr static double kPollDuration = 1.0;
   // Constants used for diagnostic frequency updater
   //! [s] 5Hz diagnostic period
@@ -579,6 +581,12 @@ class UbloxNode : public virtual ComponentInterface {
   void addProductInterface(std::string product_category,
                            std::string ref_rov = "");
 
+  /**
+   * @brief Poll version message from the U-Blox device to keep socket active.
+   * @param event a timer indicating how often to poll
+   */
+  void keepAlive(const ros::TimerEvent& event);
+
   /**
    * @brief Poll messages from the U-Blox device.
    * @param event a timer indicating how often to poll the messages

ublox_gps/src/gps.cpp

@@ -246,6 +246,42 @@ void Gps::initializeTcp(std::string host, std::string port) {
                                                     io_service)));
 }
 
+void Gps::initializeUdp(std::string host, std::string port) {
+  host_ = host;
+  port_ = port;
+  boost::shared_ptr<boost::asio::io_service> io_service(
+      new boost::asio::io_service);
+  boost::asio::ip::udp::resolver::iterator endpoint;
+
+  try {
+    boost::asio::ip::udp::resolver resolver(*io_service);
+    endpoint =
+        resolver.resolve(boost::asio::ip::udp::resolver::query(host, port));
+  } catch (std::runtime_error& e) {
+    throw std::runtime_error("U-Blox: Could not resolve" + host + " " +
+                             port + " " + e.what());
+  }
+
+  boost::shared_ptr<boost::asio::ip::udp::socket> socket(
+    new boost::asio::ip::udp::socket(*io_service));
+
+  try {
+    socket->connect(*endpoint);
+  } catch (std::runtime_error& e) {
+    throw std::runtime_error("U-Blox: Could not connect to " +
+                             endpoint->host_name() + ":" +
+                             endpoint->service_name() + ": " + e.what());
+  }
+
+  ROS_INFO("U-Blox: Connected to %s:%s.", endpoint->host_name().c_str(),
+           endpoint->service_name().c_str());
+
+  if (worker_) return;
+  setWorker(boost::shared_ptr<Worker>(
+      new AsyncWorker<boost::asio::ip::udp::socket>(socket,
+                                                    io_service)));
+}
+
 void Gps::close() {
   if(save_on_shutdown_) {
     if(saveOnShutdown())

ublox_gps/src/node.cpp

@@ -231,6 +231,11 @@ void UbloxNode::getRosParams() {
   rawDataStreamPa_.getRosParams();
 }
 
+void UbloxNode::keepAlive(const ros::TimerEvent& event) {
+  // Poll version message to keep UDP socket active
+  gps.poll(ublox_msgs::MonVER::CLASS_ID, ublox_msgs::MonVER::MESSAGE_ID);
+}
+
 void UbloxNode::pollMessages(const ros::TimerEvent& event) {
   static std::vector<uint8_t> payload(1, 1);
   if (enabled["aid_alm"])
@@ -535,6 +540,12 @@ void UbloxNode::initializeIo() {
       ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(),
                port.c_str());
       gps.initializeTcp(host, port);
+    } else if (proto == "udp") {
+      std::string host(match[2]);
+      std::string port(match[3]);
+      ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(),
+               port.c_str());
+      gps.initializeUdp(host, port);
     } else {
       throw std::runtime_error("Protocol '" + proto + "' is unsupported");
     }
@@ -573,6 +584,14 @@ void UbloxNode::initialize() {
     // Configure INF messages (needs INF params, call after subscribing)
     configureInf();
 
+    ros::Timer keep_alive;
+    if (device_.substr(0, 6) == "udp://") {
+      // Setup timer to poll version message to keep UDP socket active
+      keep_alive = nh->createTimer(ros::Duration(kKeepAlivePeriod),
+                                   &UbloxNode::keepAlive,
+                                   this);
+    }
+
     ros::Timer poller;
     poller = nh->createTimer(ros::Duration(kPollDuration),
                              &UbloxNode::pollMessages,