(Server) publish hostname for shm reconnect

rgbd/include/rgbd/server.h

@@ -5,6 +5,11 @@
 #include "rgbd/server_rgbd.h"
 #include "rgbd/server_shm.h"
 
+#include <ros/node_handle.h>
+#include <ros/publisher.h>
+
+#include <thread>
+
 namespace rgbd {
 
 class Image;
@@ -48,6 +53,20 @@ class Server {
 
     ServerSHM server_shm_;
 
+    ros::NodeHandle nh_;
+    ros::Publisher pub_shm_hostname_;
+
+    std::string hostname_;
+
+    // Publisher thread
+    std::thread pub_hostname_thread_;
+
+    /**
+     * @brief Function to publish hostname on string topic
+     * @param frequency frequency for checking service requests
+     */
+    void pubHostnameThreadFunc(const float frequency);
+
 };
 
 }

rgbd/src/server.cpp

@@ -1,17 +1,33 @@
 #include "rgbd/server.h"
+#include "rgbd/tools.h"
+
+#include <ros/console.h>
+
+#include <std_msgs/String.h>
+
+#include <exception>
 
 namespace rgbd {
 
 // ----------------------------------------------------------------------------------------
 
 Server::Server()
 {
+    const std::string& hostname = get_hostname();
+    if (hostname.empty())
+    {
+        ROS_FATAL("Can't determine hostname");
+        throw std::runtime_error("Can't determine hostname");
+    }
+    hostname_ = hostname;
 }
 
 // ----------------------------------------------------------------------------------------
 
 Server::~Server()
 {
+    nh_.shutdown();
+    pub_hostname_thread_.join();
 }
 
 // ----------------------------------------------------------------------------------------
@@ -20,6 +36,10 @@ void Server::initialize(const std::string& name, RGBStorageType rgb_type, DepthS
 {
     server_rgbd_.initialize(name, rgb_type, depth_type, service_freq);
     server_shm_.initialize(name);
+
+    pub_shm_hostname_ = nh_.advertise<std_msgs::String>(name + "/shm", 1);
+
+    pub_hostname_thread_ = std::thread(&Server::pubHostnameThreadFunc, this, 10);
 }
 
 // ----------------------------------------------------------------------------------------
@@ -28,7 +48,20 @@ void Server::send(const Image& image, bool)
 {
     server_rgbd_.send(image);
     server_shm_.send(image);
+}
+
+// ----------------------------------------------------------------------------------------
 
+void Server::pubHostnameThreadFunc(const float frequency)
+{
+    ros::Rate r(frequency);
+    std_msgs::String msg;
+    msg.data = hostname_;
+    while(nh_.ok())
+    {
+        pub_shm_hostname_.publish(msg);
+        r.sleep();
+    }
 }
 
 }