Skip to content

Commit

Permalink
Only publish hostname incase shm is really going to be opened
Browse files Browse the repository at this point in the history
  • Loading branch information
MatthijsBurgh committed Jun 9, 2020
1 parent df21740 commit b680941
Show file tree
Hide file tree
Showing 4 changed files with 21 additions and 10 deletions.
4 changes: 3 additions & 1 deletion rgbd/include/rgbd/server.h
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include <ros/node_handle.h>
#include <ros/publisher.h>

#include <memory>
#include <thread>

namespace rgbd {
Expand Down Expand Up @@ -55,10 +56,11 @@ class Server {

ros::NodeHandle nh_;

std::string name_;
std::string hostname_;

// Publisher thread
std::thread pub_hostname_thread_;
std::unique_ptr<std::thread> pub_hostname_thread_ptr_;

};

Expand Down
2 changes: 1 addition & 1 deletion rgbd/src/client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ void Client::subHostsThreadFunc(const float frequency)
{
ROS_DEBUG("Switching from ClientRGBD to ClientSHM");
client_rgbd_.deintialize();
client_shm_.intialize(server_name_, d.toSec());
client_shm_.intialize(server_name_, 0);
}
}
r.sleep();
Expand Down
10 changes: 8 additions & 2 deletions rgbd/src/rgbd_to_shm.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
#include <std_msgs/String.h>

#include <functional>
#include <memory>
#include <thread>


Expand All @@ -39,7 +40,7 @@ int main(int argc, char **argv)
server.initialize(server_name);

ros::NodeHandle nh;
std::thread pub_hostname_thread(rgbd::pubHostnameThreadFunc, std::ref(nh), server_name, host_name, 10);
std::unique_ptr<std::thread> pub_hostname_thread_ptr(nullptr);

rgbd::ImagePtr image_ptr;

Expand All @@ -48,12 +49,17 @@ int main(int argc, char **argv)
{
image_ptr = client.nextImage();
if (image_ptr)
{
if (!pub_hostname_thread_ptr)
pub_hostname_thread_ptr = std::unique_ptr<std::thread>(new std::thread(rgbd::pubHostnameThreadFunc, std::ref(nh), server_name, host_name, 10));
server.send(*image_ptr);
}
r.sleep();
}

nh.shutdown();
pub_hostname_thread.join();
if(pub_hostname_thread_ptr)
pub_hostname_thread_ptr->join();

return 0;
}
15 changes: 9 additions & 6 deletions rgbd/src/server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ namespace rgbd {

// ----------------------------------------------------------------------------------------

Server::Server()
Server::Server() : pub_hostname_thread_ptr_(nullptr)
{
const std::string& hostname = get_hostname();
hostname_ = hostname;
Expand All @@ -19,24 +19,27 @@ Server::Server()

Server::~Server()
{
ROS_WARN("Server::~Server()");
nh_.shutdown();
pub_hostname_thread_.join();
if (pub_hostname_thread_ptr_)
pub_hostname_thread_ptr_->join();
}

// ----------------------------------------------------------------------------------------

void Server::initialize(const std::string& name, RGBStorageType rgb_type, DepthStorageType depth_type, const float service_freq)
{
server_rgbd_.initialize(name, rgb_type, depth_type, service_freq);
server_shm_.initialize(name);

pub_hostname_thread_ = std::thread(rgbd::pubHostnameThreadFunc, std::ref(nh_), name, hostname_, 10);
name_= name;
server_rgbd_.initialize(name_, rgb_type, depth_type, service_freq);
server_shm_.initialize(name_);
}

// ----------------------------------------------------------------------------------------

void Server::send(const Image& image, bool)
{
if (!pub_hostname_thread_ptr_)
pub_hostname_thread_ptr_ = std::unique_ptr<std::thread>(new std::thread(rgbd::pubHostnameThreadFunc, std::ref(nh_), name_, hostname_, 10));
server_rgbd_.send(image);
server_shm_.send(image);
}
Expand Down

0 comments on commit b680941

Please sign in to comment.