Feature/reuse frame (#27)

* get frame ID from graph wrapper

* actually include frame ID in header

* fix launch files

Author: nathanhhughes

hydra_ros/launch/hydra.launch.yaml

@@ -53,7 +53,5 @@ launch:
         --config-utilities-yaml $(var extra_yaml)
   # hydra visualizer
   - set_remap: {from: hydra_visualizer/dsg, to: hydra/backend/dsg}
-  - include:
-      file: $(find-pkg-share hydra_visualizer)/launch/streaming_visualizer.launch.yaml
-      arg:
-        - {name: visualizer_frame, value: $(var map_frame)}
+  - arg: {name: start_visualizer, default: 'true', description: run hydra visualizer}
+  - include: {file: $(find-pkg-share hydra_visualizer)/launch/streaming_visualizer.launch.yaml, if: $(var start_visualizer)}

hydra_ros/src/utils/dsg_streaming_interface.cpp

@@ -123,6 +123,7 @@ void DsgSender::publishGraph(const DynamicSceneGraph& graph,
 
   auto msg = std::make_unique<DsgUpdate>();
   msg->header.stamp = rclcpp::Time(timestamp_ns);
+  msg->header.frame_id = config.frame_id;
   spark_dsg::io::binary::writeGraph(
       graph, msg->layer_contents, config.serialize_dsg_mesh);
   msg->full_update = true;

hydra_visualizer/include/hydra_visualizer/io/graph_file_wrapper.h

@@ -49,6 +49,7 @@ namespace hydra {
 class GraphFileWrapper : public GraphWrapper {
  public:
   struct Config {
+    std::string frame_id;
     std::filesystem::path filepath;
     std::string wrapper_ns = "~/graph";
   } const config;

hydra_visualizer/include/hydra_visualizer/io/graph_ros_wrapper.h

@@ -65,6 +65,7 @@ class GraphRosWrapper : public GraphWrapper {
   ianvs::NodeHandle nh_;
   rclcpp::Subscription<hydra_msgs::msg::DsgUpdate>::SharedPtr sub_;
   rclcpp::Time last_time_;
+  std::string last_frame_id_;
   spark_dsg::DynamicSceneGraph::Ptr graph_;
 };
 

hydra_visualizer/include/hydra_visualizer/io/graph_wrapper.h

@@ -41,6 +41,7 @@ namespace hydra {
 
 struct StampedGraph {
   spark_dsg::DynamicSceneGraph::Ptr graph;
+  std::string frame_id = "";
   std::optional<rclcpp::Time> timestamp = std::nullopt;
   inline operator bool() const { return graph != nullptr; }
 };

hydra_visualizer/include/hydra_visualizer/io/graph_zmq_wrapper.h

@@ -48,6 +48,7 @@ namespace hydra {
 class GraphZmqWrapper : public GraphWrapper {
  public:
   struct Config {
+    std::string frame_id;
     std::string url = "tcp://127.0.0.1:8001";
     size_t num_threads = 2;
     size_t poll_time_ms = 10;

hydra_visualizer/include/hydra_visualizer/visualizer_node.h

@@ -50,7 +50,6 @@ class DsgVisualizer : public rclcpp::Node {
  public:
   struct Config {
     double loop_period_s = 0.1;
-    std::string visualizer_frame = "map";
     SceneGraphRenderer::Config renderer;
     config::VirtualConfig<GraphWrapper> graph;
     // Specify additional plugins that should be loaded <name, config>

hydra_visualizer/launch/static_visualizer.launch.yaml

@@ -21,7 +21,7 @@ launch:
         --config-utilities-file $(var visualizer_plugins_path)
         --config-utilities-file $(var external_plugins_path)
         --config-utilities-yaml {glog_level: 0, glog_verbosity: $(var verbosity)}
-        --config-utilities-yaml {visualizer_frame: $(var visualizer_frame), graph: {type: GraphFromFile, filepath: $(var scene_graph)}}
+        --config-utilities-yaml {graph: {type: GraphFromFile, frame_id: $(var visualizer_frame), filepath: $(var scene_graph)}}
         --config-utilities-yaml {plugins: {mesh: {use_color_adapter: $(var use_color_adapter)}}}
   # rviz node and control for launching
   - arg: {name: start_rviz, default: 'true', description: automatically start rviz}

hydra_visualizer/launch/streaming_visualizer.launch.yaml

@@ -4,14 +4,14 @@ launch:
   - arg: {name: verbosity, default: '0'}
   - arg: {name: debug, default: 'false'}
   # visualizer configuration
-  - arg: {name: visualizer_frame, default: map}
   - arg: {name: visualizer_ns, default: hydra_visualizer}
   - arg: {name: visualizer_config_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_config.yaml}
   - arg: {name: visualizer_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/visualizer_plugins.yaml}
   - arg: {name: external_plugins_path, default: $(find-pkg-share hydra_visualizer)/config/external_plugins.yaml}
   - arg: {name: use_color_adapter, default: 'false', description: Start visualizer using mesh color adapter}
   # communication
   - arg: {name: use_zmq, default: 'false', description: use zmq to receive scene graphs}
+  - arg: {name: visualizer_frame, default: map, description: frame ID for zmq to use}
   - arg: {name: zmq_url, default: tcp://127.0.0.1:8001, description: full zmq url}
   # visualizer node and control for launching
   - arg: {name: start_visualizer, default: 'true'}
@@ -27,8 +27,7 @@ launch:
         --config-utilities-file $(var visualizer_plugins_path)
         --config-utilities-file $(var external_plugins_path)
         --config-utilities-yaml {glog_level: 1, glog_verbosity: $(var verbosity)}
-        --config-utilities-yaml {visualizer_frame: $(var visualizer_frame)}
-        --config-utilities-yaml {graph: {type: $(if $(var use_zmq) GraphFromZmq GraphFromRos), url: $(var zmq_url)}}
+        --config-utilities-yaml {graph: {type: $(if $(var use_zmq) GraphFromZmq GraphFromRos), url: $(var zmq_url), frame_id: $(var visualizer_frame)}}
         --config-utilities-yaml {plugins: {mesh: {use_color_adapter: $(var use_color_adapter)}}}
   # rviz node and control for launching
   - arg: {name: start_rviz, default: 'true', description: automatically start rviz}

hydra_visualizer/src/io/graph_file_wrapper.cpp

@@ -58,9 +58,11 @@ using std_srvs::srv::Empty;
 void declare_config(GraphFileWrapper::Config& config) {
   using namespace config;
   name("GraphFileWrapper::Config");
+  field(config.frame_id, "frame_id");
   field<Path::Absolute>(config.filepath, "filepath");
   field(config.wrapper_ns, "wrapper_ns");
 
+  checkCondition(!config.frame_id.empty(), "'frame_id' must be non-empty");
   check<Path::Exists>(config.filepath, "filepath");
 }
 
@@ -77,7 +79,7 @@ bool GraphFileWrapper::hasChange() const { return has_change_; }
 
 void GraphFileWrapper::clearChangeFlag() { has_change_ = false; }
 
-StampedGraph GraphFileWrapper::get() const { return {graph_}; }
+StampedGraph GraphFileWrapper::get() const { return {graph_, config.frame_id}; }
 
 void GraphFileWrapper::reload(const std_srvs::srv::Empty::Request::SharedPtr&,
                               std_srvs::srv::Empty::Response::SharedPtr) {