Got UDP connection working

roship_io/config/udp_connection.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    rx:
+      buffer_size: 1024
+      port: 1234
+    tx:
+      host: 127.0.0.1
+      port: 4321
+

roship_io/include/roship_io/connection/io_connection.hpp

@@ -0,0 +1,30 @@
+#pragma once  // Favor using this over the #ifndef, #define method
+
+
+// First include your local package stuff
+#include "connection_defs.hpp"  //  This is where we include all our namespace stuff for the package
+
+#include "transport/transport_interface.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+
+
+CONNECTION_NS_HEAD
+template <typename transport_T>
+class IoConnection
+{
+public:
+  typedef std::shared_ptr<IoConnection> SharedPtr;
+  struct Params{
+    virtual void declare(rclcpp::Node::SharedPtr node){return;}
+    virtual void update(rclcpp::Node::SharedPtr node){return;}
+  };
+
+  IoConnection(rclcpp::Node::SharedPtr node): node_ptr_(node){return;}
+protected:
+  rclcpp::Node::SharedPtr       node_ptr_;
+  std::shared_ptr<transport_T>  trasnport_ptr_;
+};
+
+CONNECTION_NS_FOOT

roship_io/include/roship_io/connection/udp_connection.hpp

@@ -0,0 +1,52 @@
+#pragma once  // Favor using this over the #ifndef, #define method
+
+
+// First include your local package stuff
+#include "connection_defs.hpp"  //  This is where we include all our namespace stuff for the package
+#include <rclcpp/rclcpp.hpp>
+#include "io_connection.hpp"
+#include "transport/udp_socket.hpp"
+
+#include <io_interfaces/msg/raw_packet.hpp>
+
+
+CONNECTION_NS_HEAD
+
+class UdpConnection : public IoConnection<transport::UdpSocket>
+{
+public:
+  struct Params
+  {
+    Params();
+    void declare(rclcpp::Node::SharedPtr node);
+    void update(rclcpp::Node::SharedPtr node);
+    struct{
+      int port;
+      std::string host;
+    } tx;
+    struct{
+      int port;
+      int buffer_size;
+    } rx;
+
+  };
+
+  UdpConnection(rclcpp::Node::SharedPtr node);
+
+  void udpCallback(const std::vector<byte>& datagram);
+
+  void sendToDevice(const io_interfaces::msg::RawPacket msg);
+
+  void spin_once();
+
+
+protected:
+  rclcpp::TimerBase::SharedPtr          timer_;
+  std::shared_ptr<transport::UdpSocket> sock_ptr_;
+  rclcpp::Publisher<io_interfaces::msg::RawPacket>::SharedPtr raw_pub_;
+  rclcpp::Subscription<io_interfaces::msg::RawPacket>::SharedPtr raw_sub_;
+  Params params_;
+
+};
+
+CONNECTION_NS_FOOT

roship_io/include/roship_io/transport/transport_interface.hpp

@@ -5,9 +5,6 @@
 
 #include <functional>
 
-
-
-
 TRANSPORT_NS_HEAD
 
 using MessageCallback = std::function<void(const std::vector<byte>&)>;

roship_io/include/roship_io/transport/trasnport_defs.hpp

@@ -6,7 +6,7 @@
 ///
 
 #define TRANSPORT_NS_HEAD  \
-  NS_HEAD namespace trasnport {
+  NS_HEAD namespace transport {
 
 #define TRANSPORT_NS_FOOT  \
   NS_FOOT }

roship_io/include/roship_io/transport/udp_socket.hpp

@@ -19,8 +19,6 @@
 
 TRANSPORT_NS_HEAD
 
-//using MessageCallback = std::function<void(const std::vector<byte>&)>;
-
 class UdpSocket : public TransportInterface {
  public:
   UdpSocket(int port, size_t buffer_size = 1024);

roship_io/launch/udp_connection.launch.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<launch>
+
+  <arg name="device_name"       default="udp_device"/>
+  <arg name="namespace"         default=""/>
+
+  <node pkg="roship_io" exec="udp_connection" name="udp_device" namespace="$(var namespace)">
+    <param from="$(find-pkg-share roship_io)/config/udp_connection.yaml"/>
+  </node>
+
+</launch>

roship_io/nodes/minimal_publisher.cpp

@@ -1,10 +1,45 @@
-#include "minimal_publisher_node.hpp"
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
 
-int main(int argc, char *argv[])
+#include "rclcpp/rclcpp.hpp"
+#include <io_interfaces/msg/raw_packet.hpp>
+
+using namespace std::chrono_literals;
+
+/* This example creates a subclass of Node and uses std::bind() to register a
+* member function as a callback from the timer. */
+
+class MinimalPublisher : public rclcpp::Node
+{
+  public:
+    MinimalPublisher()
+    : Node("minimal_publisher"), count_(0)
+    {
+      publisher_ = this->create_publisher<io_interfaces::msg::RawPacket>("udp_connection/to_device", 10);
+      timer_ = this->create_wall_timer(
+      500ms, std::bind(&MinimalPublisher::timer_callback, this));
+    }
+
+  private:
+    void timer_callback()
+    {
+      auto message = io_interfaces::msg::RawPacket();
+      std::vector<unsigned char> data = {'H','e','l','l','o'};
+      message.data = data;
+      RCLCPP_INFO(this->get_logger(), "Publishing");
+      publisher_->publish(message);
+    }
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Publisher<io_interfaces::msg::RawPacket>::SharedPtr publisher_;
+    size_t count_;
+};
+
+int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<roship_io::MinimalPublisherNode>());
+  rclcpp::spin(std::make_shared<MinimalPublisher>());
   rclcpp::shutdown();
   return 0;
-
 }

roship_io/nodes/udp_connection.cpp

@@ -0,0 +1,12 @@
+#include "connection/udp_connection.hpp"
+
+int main(int argc, char *argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr node(new rclcpp::Node("udp_connection"));
+  roship_io::connection::UdpConnection::SharedPtr connection(new roship_io::connection::UdpConnection(node));
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+
+}

roship_io/src/udp_connection.cpp

@@ -0,0 +1,79 @@
+#include "connection/udp_connection.hpp"
+
+CONNECTION_NS_HEAD
+
+using namespace std::chrono_literals;
+
+UdpConnection::Params::Params()
+{
+  rx.port = 1234;
+  rx.buffer_size = 1024;
+  tx.host = "127.0.0.1";
+  tx.port = 4321;
+}
+
+void UdpConnection::Params::declare(rclcpp::Node::SharedPtr node)
+{
+  node->declare_parameter("rx.port", rx.port);
+  node->declare_parameter("rx.buffer_size", rx.buffer_size);
+  node->declare_parameter("tx.host", tx.host);
+  node->declare_parameter("tx.port", tx.port);
+}
+
+void UdpConnection::Params::update(rclcpp::Node::SharedPtr node)
+{
+  node->get_parameter("rx.port", rx.port);
+  node->get_parameter("rx.buffer_size", rx.buffer_size);
+  node->get_parameter("tx.host", tx.host);
+  node->get_parameter("tx.port", tx.port);
+}
+
+UdpConnection::UdpConnection(rclcpp::Node::SharedPtr node):
+  IoConnection<transport::UdpSocket>(node)
+{
+  params_.declare(node_ptr_);
+  params_.update(node_ptr_);
+
+  sock_ptr_.reset(
+        new transport::UdpSocket(
+          params_.rx.port,
+          params_.rx.buffer_size)
+        );
+  sock_ptr_->AddCallback(std::bind(&UdpConnection::udpCallback,
+                                   this , std::placeholders::_1));
+
+  raw_pub_ = node_ptr_->create_publisher<io_interfaces::msg::RawPacket>("~/from_device", 10);
+
+  raw_sub_ = node_ptr_->create_subscription<io_interfaces::msg::RawPacket>(
+          "~/to_device", 1, std::bind(&UdpConnection::sendToDevice, this, std::placeholders::_1));
+
+  timer_ = node_ptr_->create_wall_timer(
+        1ms, std::bind(&UdpConnection::spin_once, this));
+
+  RCLCPP_INFO(node_ptr_->get_logger(),
+              "Listeing on port %i with buffer size %i", params_.rx.port,params_.rx.buffer_size);
+  RCLCPP_INFO(node_ptr_->get_logger(),
+              "sending message to device from topic: %s", raw_sub_->get_topic_name());
+
+}
+
+void UdpConnection::udpCallback(const std::vector<byte> &datagram)
+{
+  auto rx_time = node_ptr_->now();
+  io_interfaces::msg::RawPacket::SharedPtr msg(new io_interfaces::msg::RawPacket);
+  msg->header.stamp = rx_time;
+  msg->data = datagram;
+  raw_pub_->publish(*msg);
+}
+
+void UdpConnection::sendToDevice(const io_interfaces::msg::RawPacket msg)
+{
+  sock_ptr_->SendTo(params_.tx.host, params_.tx.port,msg.data);
+}
+
+void UdpConnection::spin_once()
+{
+  sock_ptr_->Receive();
+}
+
+CONNECTION_NS_FOOT