From b491783417041f2da1efef3b3374c9cf88fd41df Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jo=C3=A3o=20Reis?= <joaocgreis@gmail.com>
Date: Sun, 16 Nov 2014 03:10:04 +0000
Subject: [PATCH 1/2] Improved the ROS interface

---
 include/ros_roah_rsbb.h | 215 +++++++++++++++++++++++++++++++++++++++-
 1 file changed, 213 insertions(+), 2 deletions(-)

diff --git a/include/ros_roah_rsbb.h b/include/ros_roah_rsbb.h
index 02459f2..96f30e4 100644
--- a/include/ros_roah_rsbb.h
+++ b/include/ros_roah_rsbb.h
@@ -21,10 +21,16 @@
 #define __ROS_ROAH_RSBB_H__
 
 #include <string>
+#include <memory>
+
+#include <boost/make_shared.hpp>
 
 #include <ros/console.h>
 #include <ros/param.h>
 #include <ros/time.h>
+#include <ros/init.h>
+#include <ros/callback_queue.h>
+#include <ros/callback_queue_interface.h>
 
 #include "roah_rsbb.h"
 
@@ -93,8 +99,213 @@ namespace roah_rsbb
       }
   };
 
-  typedef PublicChannel<RosErrorHandler> RosPublicChannel;
-  typedef PrivateChannel<RosErrorHandler> RosPrivateChannel;
+
+
+  class CallbackItem
+    : public ros::CallbackInterface
+  {
+      boost::function<void() > f_;
+
+    public:
+      CallbackItem (boost::function<void() > const& f)
+        : f_ (f)
+      {
+      }
+
+      virtual CallResult call()
+      {
+        f_();
+        return Success;
+      }
+  };
+
+
+
+  class RosPublicChannel
+    : public PublicChannel<RosErrorHandler>
+  {
+      boost::function<void (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RoahRsbbBeacon>) > rsbb_beacon_callback_;
+      boost::function<void (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RobotBeacon>) > robot_beacon_callback_;
+      boost::function<void (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::TabletBeacon>) > tablet_beacon_callback_;
+
+      void
+      receive_rsbb_beacon (boost::asio::ip::udp::endpoint& endpoint,
+                           uint16_t comp_id,
+                           uint16_t msg_type,
+                           std::shared_ptr<const roah_rsbb_msgs::RoahRsbbBeacon> msg)
+      {
+        ROS_DEBUG_STREAM ("Received RoahRsbbBeacon from " << endpoint.address().to_string()
+                          << ":" << endpoint.port()
+                          << ", COMP_ID " << comp_id
+                          << ", MSG_TYPE " << msg_type);
+
+        if (rsbb_beacon_callback_) {
+          ros::getGlobalCallbackQueue()->addCallback (boost::make_shared<CallbackItem> (boost::bind (rsbb_beacon_callback_, endpoint, comp_id, msg_type, msg)));
+        }
+      }
+
+      void
+      receive_robot_beacon (boost::asio::ip::udp::endpoint& endpoint,
+                            uint16_t comp_id,
+                            uint16_t msg_type,
+                            std::shared_ptr<const roah_rsbb_msgs::RobotBeacon> msg)
+      {
+        ROS_DEBUG_STREAM ("Received RobotBeacon from " << endpoint.address().to_string()
+                          << ":" << endpoint.port()
+                          << ", COMP_ID " << comp_id
+                          << ", MSG_TYPE " << msg_type
+                          << ", team_name: " << msg->team_name()
+                          << ", robot_name: " << msg->robot_name());
+
+        if (robot_beacon_callback_) {
+          ros::getGlobalCallbackQueue()->addCallback (boost::make_shared<CallbackItem> (boost::bind (robot_beacon_callback_, endpoint, comp_id, msg_type, msg)));
+        }
+      }
+
+      void
+      receive_tablet_beacon (boost::asio::ip::udp::endpoint& endpoint,
+                             uint16_t comp_id,
+                             uint16_t msg_type,
+                             std::shared_ptr<const roah_rsbb_msgs::TabletBeacon> msg)
+      {
+        ROS_DEBUG_STREAM ("Received TabletBeacon from " << endpoint.address().to_string()
+                          << ":" << endpoint.port()
+                          << ", COMP_ID " << comp_id
+                          << ", MSG_TYPE " << msg_type);
+
+        if (tablet_beacon_callback_) {
+          ros::getGlobalCallbackQueue()->addCallback (boost::make_shared<CallbackItem> (boost::bind (tablet_beacon_callback_, endpoint, comp_id, msg_type, msg)));
+        }
+      }
+
+    public:
+      RosPublicChannel (std::string const& host,
+                        unsigned short port)
+        : PublicChannel<RosErrorHandler> (host, port)
+      {
+        signal_rsbb_beacon_received().connect (boost::bind (&RosPublicChannel::receive_rsbb_beacon, this, _1, _2, _3, _4));
+        signal_robot_beacon_received().connect (boost::bind (&RosPublicChannel::receive_robot_beacon, this, _1, _2, _3, _4));
+        signal_tablet_beacon_received().connect (boost::bind (&RosPublicChannel::receive_tablet_beacon, this, _1, _2, _3, _4));
+      }
+
+      void
+      set_rsbb_beacon_callback (boost::function<void (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RoahRsbbBeacon>) > rsbb_beacon_callback)
+      {
+        rsbb_beacon_callback_ = rsbb_beacon_callback;
+      }
+
+      template<class T> void
+      set_rsbb_beacon_callback (void (T::*callback) (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RoahRsbbBeacon>),
+                                T* obj)
+      {
+        return set_rsbb_beacon_callback (boost::bind (callback, obj, _1, _2, _3, _4));
+      }
+
+      void
+      set_robot_beacon_callback (boost::function<void (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RobotBeacon>) > robot_beacon_callback)
+      {
+        robot_beacon_callback_ = robot_beacon_callback;
+      }
+
+      template<class T> void
+      set_robot_beacon_callback (void (T::*callback) (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RobotBeacon>),
+                                 T* obj)
+      {
+        return set_robot_beacon_callback (boost::bind (callback, obj, _1, _2, _3, _4));
+      }
+
+      void
+      set_tablet_beacon_callback (boost::function<void (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::TabletBeacon>) > tablet_beacon_callback)
+      {
+        tablet_beacon_callback_ = tablet_beacon_callback;
+      }
+
+      template<class T> void
+      set_tablet_beacon_callback (void (T::*callback) (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::TabletBeacon>),
+                                  T* obj)
+      {
+        return set_tablet_beacon_callback (boost::bind (callback, obj, _1, _2, _3, _4));
+      }
+  };
+
+
+
+  class RosPrivateChannel
+    : public PrivateChannel<RosErrorHandler>
+  {
+      boost::function<void (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::BenchmarkState>) > benchmark_state_callback_;
+      boost::function<void (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RobotState>) > robot_state_callback_;
+
+      void
+      receive_benchmark_state (boost::asio::ip::udp::endpoint& endpoint,
+                               uint16_t comp_id,
+                               uint16_t msg_type,
+                               std::shared_ptr<const roah_rsbb_msgs::BenchmarkState> msg)
+      {
+        ROS_DEBUG_STREAM ("Received BenchmarkState from " << endpoint.address().to_string()
+                          << ":" << endpoint.port()
+                          << ", COMP_ID " << comp_id
+                          << ", MSG_TYPE " << msg_type
+                          << ", benchmark_type: " << msg->benchmark_type());
+
+        if (benchmark_state_callback_) {
+          ros::getGlobalCallbackQueue()->addCallback (boost::make_shared<CallbackItem> (boost::bind (benchmark_state_callback_, endpoint, comp_id, msg_type, msg)));
+        }
+      }
+
+      void
+      receive_robot_state (boost::asio::ip::udp::endpoint& endpoint,
+                           uint16_t comp_id,
+                           uint16_t msg_type,
+                           std::shared_ptr<const roah_rsbb_msgs::RobotState> msg)
+      {
+        ROS_DEBUG_STREAM ("Received RobotState from " << endpoint.address().to_string()
+                          << ":" << endpoint.port()
+                          << ", COMP_ID " << comp_id
+                          << ", MSG_TYPE " << msg_type);
+
+        if (robot_state_callback_) {
+          ros::getGlobalCallbackQueue()->addCallback (boost::make_shared<CallbackItem> (boost::bind (robot_state_callback_, endpoint, comp_id, msg_type, msg)));
+        }
+      }
+
+    public:
+      RosPrivateChannel (std::string const& host,
+                         unsigned short port,
+                         std::string const& key,
+                         std::string const& cypher)
+        : PrivateChannel<RosErrorHandler> (host, port, key, cypher)
+      {
+        signal_benchmark_state_received().connect (boost::bind (&RosPrivateChannel::receive_benchmark_state, this, _1, _2, _3, _4));
+        signal_robot_state_received().connect (boost::bind (&RosPrivateChannel::receive_robot_state, this, _1, _2, _3, _4));
+      }
+
+      void
+      set_benchmark_state_callback (boost::function<void (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::BenchmarkState>) > benchmark_state_callback)
+      {
+        benchmark_state_callback_ = benchmark_state_callback;
+      }
+
+      template<class T> void
+      set_benchmark_state_callback (void (T::*callback) (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::BenchmarkState>),
+                                    T* obj)
+      {
+        return set_benchmark_state_callback (boost::bind (callback, obj, _1, _2, _3, _4));
+      }
+
+      void
+      set_robot_state_callback (boost::function<void (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RobotState>) > robot_state_callback)
+      {
+        robot_state_callback_ = robot_state_callback;
+      }
+
+      template<class T> void
+      set_robot_state_callback (void (T::*callback) (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RobotState>),
+                                T* obj)
+      {
+        return set_robot_state_callback (boost::bind (callback, obj, _1, _2, _3, _4));
+      }
+  };
 }
 
 

From 7d842086f211dbc6b843aefb233f4fac02764a4e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jo=C3=A3o=20Reis?= <joaocgreis@gmail.com>
Date: Fri, 21 Nov 2014 09:55:00 +0000
Subject: [PATCH 2/2] Removed redundant return

---
 include/ros_roah_rsbb.h | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/include/ros_roah_rsbb.h b/include/ros_roah_rsbb.h
index 96f30e4..d7c07a6 100644
--- a/include/ros_roah_rsbb.h
+++ b/include/ros_roah_rsbb.h
@@ -198,7 +198,7 @@ namespace roah_rsbb
       set_rsbb_beacon_callback (void (T::*callback) (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RoahRsbbBeacon>),
                                 T* obj)
       {
-        return set_rsbb_beacon_callback (boost::bind (callback, obj, _1, _2, _3, _4));
+        set_rsbb_beacon_callback (boost::bind (callback, obj, _1, _2, _3, _4));
       }
 
       void
@@ -211,7 +211,7 @@ namespace roah_rsbb
       set_robot_beacon_callback (void (T::*callback) (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RobotBeacon>),
                                  T* obj)
       {
-        return set_robot_beacon_callback (boost::bind (callback, obj, _1, _2, _3, _4));
+        set_robot_beacon_callback (boost::bind (callback, obj, _1, _2, _3, _4));
       }
 
       void
@@ -224,7 +224,7 @@ namespace roah_rsbb
       set_tablet_beacon_callback (void (T::*callback) (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::TabletBeacon>),
                                   T* obj)
       {
-        return set_tablet_beacon_callback (boost::bind (callback, obj, _1, _2, _3, _4));
+        set_tablet_beacon_callback (boost::bind (callback, obj, _1, _2, _3, _4));
       }
   };
 
@@ -290,7 +290,7 @@ namespace roah_rsbb
       set_benchmark_state_callback (void (T::*callback) (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::BenchmarkState>),
                                     T* obj)
       {
-        return set_benchmark_state_callback (boost::bind (callback, obj, _1, _2, _3, _4));
+        set_benchmark_state_callback (boost::bind (callback, obj, _1, _2, _3, _4));
       }
 
       void
@@ -303,7 +303,7 @@ namespace roah_rsbb
       set_robot_state_callback (void (T::*callback) (boost::asio::ip::udp::endpoint, uint16_t, uint16_t, std::shared_ptr<const roah_rsbb_msgs::RobotState>),
                                 T* obj)
       {
-        return set_robot_state_callback (boost::bind (callback, obj, _1, _2, _3, _4));
+        set_robot_state_callback (boost::bind (callback, obj, _1, _2, _3, _4));
       }
   };
 }