Feature/hydra object rebase (#73)

* ability to parse instance id to map to mesh color

* instance color does not get colored for graphs

* add frame to mesh delta

* fix agent node settings

* cherry-pick mutex changes

* drop instance ID config fields

* rework mutex

* rename field

---------

Co-authored-by: Multyxu <multyxu@gmail.com>

Author: nathanhhughes

hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h

@@ -49,14 +49,22 @@ namespace hydra {
 class DsgSender {
  public:
   struct Config {
+    //! Frame ID to publish scene graph under
     std::string frame_id;
+    //! Timer name to record serialization timing
     std::string timer_name = "publish_dsg";
+    //! Include mesh in serialized scene graph message
     bool serialize_dsg_mesh = true;
+    //! Publish mesh separately from scene graph
     bool publish_mesh = false;
+    //! Minimum amount of time separating mesh messages
     double min_mesh_separation_s = 0.0;
+    //! Minimum amount of time separating scene graph messages
     double min_dsg_separation_s = 0.0;
 
+    //! Create a config with a specific timer name
     Config with_name(const std::string& name) const;
+    //! Create a config with a specific frame ID
     Config with_frame(const std::string& frame) const;
   } const config;
 

hydra_ros/src/frontend/ros_frontend_publisher.cpp

@@ -94,6 +94,7 @@ void RosFrontendPublisher::call(uint64_t timestamp_ns,
     backend_input.mesh_update->timestamp_ns = timestamp_ns;
     auto delta_msg = std::make_shared<MeshDeltaMsg>();
     kimera_pgmo::conversions::to_ros(*backend_input.mesh_update, *delta_msg);
+    delta_msg->header.frame_id = GlobalInfo::instance().getFrames().odom;
     mesh_update_pub_->publish(*delta_msg);
 
     stored_delta_.insert({backend_input.mesh_update->info.sequence_number, delta_msg});

hydra_visualizer/config/visualizer_config.yaml

@@ -42,8 +42,8 @@ renderer:
     2p*:
       z_offset_scale: 0.0
       visualize: true
-      nodes: {scale: 0.15, alpha: 0.9, use_sphere: false, color: {type: PartitionColorAdapter}}
-      edges: {scale: 0.05, alpha: 0.9, draw_interlayer: false}
+      nodes: {scale: 0.15, alpha: 1.0, use_sphere: true, color: {type: PartitionColorAdapter}}
+      edges: {scale: 0.15, alpha: 1.0, draw_interlayer: false, color: {type: ''}}
       text: {draw_layer: true, height: 0.9, scale: 0.8}
     3p1:
       z_offset_scale: 3.0

hydra_visualizer/include/hydra_visualizer/io/graph_ros_wrapper.h

@@ -64,8 +64,10 @@ class GraphRosWrapper : public GraphWrapper {
   bool has_change_;
   ianvs::NodeHandle nh_;
   rclcpp::Subscription<hydra_msgs::msg::DsgUpdate>::SharedPtr sub_;
+
   rclcpp::Time last_time_;
   std::string last_frame_id_;
+  mutable std::mutex graph_mutex_;
   spark_dsg::DynamicSceneGraph::Ptr graph_;
 };
 

hydra_visualizer/include/hydra_visualizer/io/graph_wrapper.h

@@ -35,14 +35,23 @@
 #pragma once
 #include <spark_dsg/dynamic_scene_graph.h>
 
+#include <mutex>
+
 #include <rclcpp/time.hpp>
 
 namespace hydra {
 
 struct StampedGraph {
+  //! Underlying scene graph
   spark_dsg::DynamicSceneGraph::Ptr graph;
+  //! Frame ID for scene graph
   std::string frame_id = "";
+  //! Timestamp of scene graph
   std::optional<rclcpp::Time> timestamp = std::nullopt;
+  //! Optional lock object for scene graph (for ROS and ZMQ wrappers)
+  std::unique_lock<std::mutex> lock = {};
+
+  //! Whether or not the scene graph is valid
   inline operator bool() const { return graph != nullptr; }
 };
 

hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h

@@ -52,6 +52,7 @@ class MeshPlugin : public VisualizerPlugin {
 
   struct Config {
     config::VirtualConfig<MeshColoring, true> coloring;
+    double min_mesh_separation_s = 0.0;
   };
 
   MeshPlugin(const Config& config, ianvs::NodeHandle nh, const std::string& name);
@@ -70,6 +71,7 @@ class MeshPlugin : public VisualizerPlugin {
 
   std::string getMsgNamespace() const;
 
+  std::optional<rclcpp::Time> last_pub_;
   rclcpp::Publisher<kimera_pgmo_msgs::msg::Mesh>::SharedPtr mesh_pub_;
 };
 

hydra_visualizer/src/io/graph_ros_wrapper.cpp

@@ -66,15 +66,21 @@ bool GraphRosWrapper::hasChange() const { return has_change_; }
 void GraphRosWrapper::clearChangeFlag() { has_change_ = false; }
 
 StampedGraph GraphRosWrapper::get() const {
-  return {graph_, last_frame_id_, last_time_};
+  // lock and pass information to the visualizer
+  std::unique_lock<std::mutex> lock(graph_mutex_);
+  return {graph_, last_frame_id_, last_time_, std::move(lock)};
 }
 
 void GraphRosWrapper::callback(const DsgUpdate::ConstSharedPtr& msg) {
-  // not designed to be threadsafe; should lock and clone graph on return if desired
+  // technically we shouldn't need a mutex here because the wrapper subscriber should be
+  // in the same callback group as the rest of the node, but easy enough to pass a lock
+  // to the visualizer
+  std::lock_guard<std::mutex> lock(graph_mutex_);
+
   last_time_ = msg->header.stamp;
   last_frame_id_ = msg->header.frame_id;
   if (last_frame_id_.empty()) {
-    LOG(ERROR) << "Received scene grpah with empty frame_id field!";
+    LOG(ERROR) << "Received scene graph with empty frame_id field!";
     return;
   }
 

hydra_visualizer/src/io/graph_zmq_wrapper.cpp

@@ -87,12 +87,13 @@ void GraphZmqWrapper::clearChangeFlag() {
 }
 
 StampedGraph GraphZmqWrapper::get() const {
-  std::lock_guard<std::mutex> lock(graph_mutex_);
+  std::unique_lock<std::mutex> lock(graph_mutex_);
   if (!graph_) {
     return {nullptr};
   }
 
-  return {graph_->clone(), config.frame_id};
+  // pass the lock for the visualizer to use the graph
+  return {graph_, config.frame_id, std::nullopt, std::move(lock)};
 }
 
 void GraphZmqWrapper::spin() {
@@ -106,6 +107,7 @@ void GraphZmqWrapper::spin() {
     if (receiver_->graph()) {
       graph_ = receiver_->graph()->clone();
     }
+
     has_change_ = true;
     VLOG(1) << "Got graph!";
   }

hydra_visualizer/src/plugins/mesh_plugin.cpp

@@ -53,22 +53,25 @@ static const auto registration =
 
 }
 
+using MeshMsg = kimera_pgmo_msgs::msg::Mesh;
+
+using config::checkValid;
 using spark_dsg::DynamicSceneGraph;
+using visualizer::makeMeshMsg;
 
 void declare_config(MeshPlugin::Config& config) {
   using namespace config;
   name("MeshPlugin::Config");
   field(config.coloring, "coloring");
+  field(config.min_mesh_separation_s, "min_mesh_separation_s", "s");
 }
 
 MeshPlugin::MeshPlugin(const Config& config,
                        ianvs::NodeHandle nh,
                        const std::string& name)
     : VisualizerPlugin(name),
-      config_(
-          "mesh_plugin", config::checkValid(config), [this]() { has_change_ = true; }),
-      mesh_pub_(nh.create_publisher<kimera_pgmo_msgs::msg::Mesh>(
-          name, rclcpp::QoS(1).transient_local())) {}
+      config_("mesh_plugin", checkValid(config), [this]() { has_change_ = true; }),
+      mesh_pub_(nh.create_publisher<MeshMsg>(name, rclcpp::QoS(1).transient_local())) {}
 
 MeshPlugin::~MeshPlugin() {}
 
@@ -80,9 +83,13 @@ void MeshPlugin::draw(const std_msgs::msg::Header& header,
   }
 
   const auto config = config_.get();
+  const rclcpp::Time now(header.stamp);
+  if (last_pub_ && (now - *last_pub_).seconds() < config.min_mesh_separation_s) {
+    return;
+  }
 
-  auto msg = visualizer::makeMeshMsg(
-      header, *mesh, getMsgNamespace(), config.coloring.create());
+  last_pub_ = now;
+  auto msg = makeMeshMsg(header, *mesh, getMsgNamespace(), config.coloring.create());
   mesh_pub_->publish(msg);
 }
 

hydra_visualizer/src/visualizer_node.cpp

@@ -122,21 +122,22 @@ void DsgVisualizer::spinOnce(bool force) {
     return;
   }
 
-  const auto stamped_graph = graph_->get();
-  if (!stamped_graph) {
-    return;
-  }
-
-  std_msgs::msg::Header header;
-  header.frame_id = stamped_graph.frame_id;
-  header.stamp = stamped_graph.timestamp.value_or(nh_.now());
+  {  // start scope for stamped graph (for optional critical region)
+    const auto stamped_graph = graph_->get();
+    if (!stamped_graph) {
+      return;
+    }
 
-  renderer_->draw(header, *stamped_graph.graph);
-  for (const auto& plugin : plugins_) {
-    if (plugin) {
-      plugin->draw(header, *stamped_graph.graph);
+    std_msgs::msg::Header header;
+    header.frame_id = stamped_graph.frame_id;
+    header.stamp = stamped_graph.timestamp.value_or(nh_.now());
+    renderer_->draw(header, *stamped_graph.graph);
+    for (const auto& plugin : plugins_) {
+      if (plugin) {
+        plugin->draw(header, *stamped_graph.graph);
+      }
     }
-  }
+  }  // end scope for stamped graph
 
   graph_->clearChangeFlag();
   renderer_->clearChangeFlag();