Feature/fix node handle (#34)

* switch to ianvs glog sink and remove factory headers

* use new node handle functionality in ianvs

* (wip) start moving visualizer to new node handle infrastructure

* (wip) disable config server for now

* readd config server

* revert glog changes

Author: nathanhhughes

hydra_ros/CMakeLists.txt

@@ -32,7 +32,6 @@ ament_python_install_package(${PROJECT_NAME})
 
 add_library(
   ${PROJECT_NAME}
-  src/common.cpp
   src/hydra_ros_pipeline.cpp
   src/active_window/reconstruction_visualizer.cpp
   src/active_window/tsdf_occupancy_publisher.cpp

hydra_ros/app/hydra_node.cpp

@@ -40,7 +40,7 @@
 #include <config_utilities/printing.h>
 #include <config_utilities/types/path.h>
 #include <hydra/common/global_info.h>
-#include <ianvs/node_handle_factory.h>
+#include <ianvs/node_init.h>
 #include <ianvs/spin_functions.h>
 
 #include "hydra_ros/hydra_ros_pipeline.h"
@@ -113,8 +113,6 @@ struct RosSink : google::LogSink {
 int main(int argc, char* argv[]) {
   config::initContext(argc, argv, true);
   config::setConfigSettingsFromContext();
-  rclcpp::init(argc, argv);
-
   const auto settings = config::fromContext<hydra::RunSettings>();
 
   FLAGS_minloglevel = settings.glog_level;
@@ -125,11 +123,12 @@ int main(int argc, char* argv[]) {
   google::InitGoogleLogging(argv[0]);
   google::InstallFailureSignalHandler();
 
-  auto node = std::make_shared<rclcpp::Node>("hydra_ros_node");
+  [[maybe_unused]] const auto node = ianvs::init_node(argc, argv, "hydra_ros_node");
+  auto nh = ianvs::NodeHandle::this_node();
 
   std::shared_ptr<hydra::RosSink> ros_sink;
   if (settings.forward_glog_to_ros) {
-    ros_sink = std::make_shared<hydra::RosSink>(node->get_logger());
+    ros_sink = std::make_shared<hydra::RosSink>(nh.logger());
     google::AddLogSink(ros_sink.get());
   }
 
@@ -138,11 +137,8 @@ int main(int argc, char* argv[]) {
 
   [[maybe_unused]] const auto plugins = config::loadExternalFactories(settings.paths);
 
-  ianvs::NodeHandle nh(*node);
-  ianvs::NodeHandleFactory::addNode("hydra_ros_node", *node);
-  hydra::GlobalInfo::instance().setForceShutdown(settings.force_shutdown);
-
   {  // start hydra scope
+    hydra::GlobalInfo::instance().setForceShutdown(settings.force_shutdown);
     hydra::HydraRosPipeline hydra(settings.robot_id, settings.config_verbosity);
     hydra.init();
 

hydra_ros/include/hydra_ros/common.h

@@ -41,7 +41,6 @@
 #include <semantic_inference_msgs/msg/feature_image.hpp>
 #include <sensor_msgs/msg/image.hpp>
 
-#include "hydra_ros/common.h"
 #include "hydra_ros/input/ros_data_receiver.h"
 
 namespace hydra {
@@ -155,9 +154,9 @@ ImageReceiverImpl<SemanticT>::ImageReceiverImpl(const Config& config,
 
 template <typename SemanticT>
 bool ImageReceiverImpl<SemanticT>::initImpl() {
-  color_sub_ = ColorSubscriber(getHydraNodeHandle(ns_));
-  depth_sub_ = DepthSubscriber(getHydraNodeHandle(ns_));
-  semantic_sub_ = SemanticT(getHydraNodeHandle(ns_));
+  color_sub_ = ColorSubscriber(ianvs::NodeHandle::this_node(ns_));
+  depth_sub_ = DepthSubscriber(ianvs::NodeHandle::this_node(ns_));
+  semantic_sub_ = SemanticT(ianvs::NodeHandle::this_node(ns_));
   sync_.reset(new Synchronizer(Policy(config.queue_size),
                                color_sub_.getFilter(),
                                depth_sub_.getFilter(),

hydra_ros/include/hydra_ros/input/image_receiver.h

@@ -39,11 +39,9 @@
 #include <hydra/common/global_info.h>
 #include <hydra_visualizer/color/color_parsing.h>
 #include <hydra_visualizer/drawing.h>
-#include <ianvs/node_handle_factory.h>
 
 #include <tf2_eigen/tf2_eigen.hpp>
 
-#include "hydra_ros/common.h"
 #include "hydra_ros/visualizer/voxel_drawing.h"
 
 namespace hydra {
@@ -108,7 +106,7 @@ void declare_config(ReconstructionVisualizer::Config& config) {
 
 ReconstructionVisualizer::ReconstructionVisualizer(const Config& config)
     : config(config),
-      nh_(getHydraNodeHandle(config.ns)),
+      nh_(ianvs::NodeHandle::this_node(config.ns)),
       pubs_(nh_),
       active_mesh_pub_(nh_.create_publisher<kimera_pgmo_msgs::msg::Mesh>("mesh", 1)),
       pose_pub_(nh_.create_publisher<geometry_msgs::msg::PoseStamped>("pose", 10)),

hydra_ros/src/active_window/reconstruction_visualizer.cpp

@@ -39,8 +39,6 @@
 #include <config_utilities/printing.h>
 #include <config_utilities/validation.h>
 
-#include "hydra_ros/common.h"
-
 namespace hydra {
 namespace voxel_traits {
 
@@ -64,7 +62,7 @@ void declare_config(TsdfOccupancyPublisher::Config& config) {
 }
 
 TsdfOccupancyPublisher::TsdfOccupancyPublisher(const Config& config)
-    : OccupancyPublisher(config, getHydraNodeHandle(config.ns)),
+    : OccupancyPublisher(config, ianvs::NodeHandle::this_node(config.ns)),
       config(config::checkValid(config)) {}
 
 std::string TsdfOccupancyPublisher::printInfo() const {

hydra_ros/src/active_window/tsdf_occupancy_publisher.cpp

@@ -39,8 +39,6 @@
 #include <config_utilities/printing.h>
 #include <config_utilities/validation.h>
 
-#include "hydra_ros/common.h"
-
 namespace hydra {
 namespace voxel_traits {
 
@@ -64,7 +62,8 @@ void declare_config(GvdOccupancyPublisher::Config& config) {
 }
 
 GvdOccupancyPublisher::GvdOccupancyPublisher(const Config& config)
-    : OccupancyPublisher<places::GvdBlock>(config, getHydraNodeHandle(config.ns)),
+    : OccupancyPublisher<places::GvdBlock>(config,
+                                           ianvs::NodeHandle::this_node(config.ns)),
       config(config::checkValid(config)) {}
 
 std::string GvdOccupancyPublisher::printInfo() const {

hydra_ros/src/common.cpp

@@ -39,8 +39,6 @@
 #include <config_utilities/validation.h>
 #include <hydra/common/global_info.h>
 
-#include "hydra_ros/common.h"
-
 namespace hydra {
 
 using visualization_msgs::msg::Marker;
@@ -56,7 +54,7 @@ void declare_config(ObjectVisualizer::Config& config) {
 
 ObjectVisualizer::ObjectVisualizer(const Config& config)
     : config(config::checkValid(config)),
-      nh_(getHydraNodeHandle(config.module_ns)),
+      nh_(ianvs::NodeHandle::this_node(config.module_ns)),
       pubs_(nh_) {}
 
 std::string ObjectVisualizer::printInfo() const { return config::toString(config); }

hydra_ros/src/frontend/gvd_occupancy_publisher.cpp

@@ -44,8 +44,6 @@
 
 #include <rclcpp/time.hpp>
 
-#include "hydra_ros/common.h"
-#include "hydra_ros/frontend/gvd_visualization_utilities.h"
 #include "hydra_ros/visualizer/voxel_drawing.h"
 
 namespace hydra {
@@ -70,7 +68,7 @@ void declare_config(PlacesVisualizer::Config& config) {
 
 PlacesVisualizer::PlacesVisualizer(const Config& config)
     : config(config),
-      nh_(getHydraNodeHandle(config.ns)),
+      nh_(ianvs::NodeHandle::this_node(config.ns)),
       pubs_(nh_),
       gvd_config_("gvd"),
       layer_config_("graph"),