diff --git a/hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h b/hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h index c37fdd4e..f3b0da95 100644 --- a/hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h +++ b/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; diff --git a/hydra_ros/src/frontend/ros_frontend_publisher.cpp b/hydra_ros/src/frontend/ros_frontend_publisher.cpp index 732921d4..5a25c9d8 100644 --- a/hydra_ros/src/frontend/ros_frontend_publisher.cpp +++ b/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(); 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}); diff --git a/hydra_visualizer/config/visualizer_config.yaml b/hydra_visualizer/config/visualizer_config.yaml index 65ce11d8..b7ffe8df 100644 --- a/hydra_visualizer/config/visualizer_config.yaml +++ b/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 diff --git a/hydra_visualizer/include/hydra_visualizer/io/graph_ros_wrapper.h b/hydra_visualizer/include/hydra_visualizer/io/graph_ros_wrapper.h index c26e05ae..558fad25 100644 --- a/hydra_visualizer/include/hydra_visualizer/io/graph_ros_wrapper.h +++ b/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::SharedPtr sub_; + rclcpp::Time last_time_; std::string last_frame_id_; + mutable std::mutex graph_mutex_; spark_dsg::DynamicSceneGraph::Ptr graph_; }; diff --git a/hydra_visualizer/include/hydra_visualizer/io/graph_wrapper.h b/hydra_visualizer/include/hydra_visualizer/io/graph_wrapper.h index d1ce8fe5..8adf8c1a 100644 --- a/hydra_visualizer/include/hydra_visualizer/io/graph_wrapper.h +++ b/hydra_visualizer/include/hydra_visualizer/io/graph_wrapper.h @@ -35,14 +35,23 @@ #pragma once #include +#include + #include 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 timestamp = std::nullopt; + //! Optional lock object for scene graph (for ROS and ZMQ wrappers) + std::unique_lock lock = {}; + + //! Whether or not the scene graph is valid inline operator bool() const { return graph != nullptr; } }; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h index 6f2b4357..07e1c4ea 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h @@ -52,6 +52,7 @@ class MeshPlugin : public VisualizerPlugin { struct Config { config::VirtualConfig 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 last_pub_; rclcpp::Publisher::SharedPtr mesh_pub_; }; diff --git a/hydra_visualizer/src/io/graph_ros_wrapper.cpp b/hydra_visualizer/src/io/graph_ros_wrapper.cpp index 6a44eca9..c568b9d5 100644 --- a/hydra_visualizer/src/io/graph_ros_wrapper.cpp +++ b/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 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 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; } diff --git a/hydra_visualizer/src/io/graph_zmq_wrapper.cpp b/hydra_visualizer/src/io/graph_zmq_wrapper.cpp index 06f9ab3e..f31e5446 100644 --- a/hydra_visualizer/src/io/graph_zmq_wrapper.cpp +++ b/hydra_visualizer/src/io/graph_zmq_wrapper.cpp @@ -87,12 +87,13 @@ void GraphZmqWrapper::clearChangeFlag() { } StampedGraph GraphZmqWrapper::get() const { - std::lock_guard lock(graph_mutex_); + std::unique_lock 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!"; } diff --git a/hydra_visualizer/src/plugins/mesh_plugin.cpp b/hydra_visualizer/src/plugins/mesh_plugin.cpp index 322b097b..22fb5258 100644 --- a/hydra_visualizer/src/plugins/mesh_plugin.cpp +++ b/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( - name, rclcpp::QoS(1).transient_local())) {} + config_("mesh_plugin", checkValid(config), [this]() { has_change_ = true; }), + mesh_pub_(nh.create_publisher(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); } diff --git a/hydra_visualizer/src/visualizer_node.cpp b/hydra_visualizer/src/visualizer_node.cpp index 15a54ed5..02501bea 100644 --- a/hydra_visualizer/src/visualizer_node.cpp +++ b/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();