diff --git a/hydra_ros/src/frontend/traversability_visualizer.cpp b/hydra_ros/src/frontend/traversability_visualizer.cpp index 968edcc..85bd0ff 100644 --- a/hydra_ros/src/frontend/traversability_visualizer.cpp +++ b/hydra_ros/src/frontend/traversability_visualizer.cpp @@ -70,7 +70,7 @@ TraversabilityVisualizer::TraversabilityVisualizer(const Config& config) config::checkValid(config), [this]() { onConfigUpdate(); }), nh_(ianvs::NodeHandle::this_node(config.ns)), - layer_pub_(nh_.create_publisher("layer", 10)) { + layer_pub_(nh_.create_publisher("layer", 100)) { onConfigUpdate(); } diff --git a/hydra_visualizer/config/visualizer_config.yaml b/hydra_visualizer/config/visualizer_config.yaml index 35571f9..bedeff7 100644 --- a/hydra_visualizer/config/visualizer_config.yaml +++ b/hydra_visualizer/config/visualizer_config.yaml @@ -9,7 +9,7 @@ renderer: nodes: {scale: 0.40, color: {type: LabelColorAdapter}, alpha: 0.8, use_sphere: false} text: {draw: true, collapse: true, adapter: {type: LabelTextAdapter}, height: 0.5, scale: 0.45} bounding_boxes: {draw: true, collapse: true, scale: 0.05, edge_scale: 0.05, alpha: 0.9, edge_break_ratio: 0.5} - edges: {interlayer_use_source: true, interlayer_scale: 0.08, interlayer_alpha: 0.9} + edges: {interlayer_use_source: true, interlayer_scale: 0.08, interlayer_alpha: 0.5} 3: z_offset_scale: 3.0 visualize: true @@ -47,10 +47,11 @@ renderer: visualize: true nodes: {scale: 0.2, color: {type: LabelColorAdapter}, alpha: 0.9, use_sphere: true} boundaries: {draw: true, collapse: false, wireframe_scale: 0.1, use_node_color: true, alpha: 1.0} + text: {draw: true, collapse: true, adapter: {type: IdTextAdapter}, height: 0.5, scale: 0.45} edges: - scale: 0.01 - alpha: 0.5 - color: {type: UniformEdgeColorAdapter} + scale: 0.1 + alpha: 0.8 + color: {type: TraversabilityEdgeColorAdapter} interlayer_use_source: false interlayer_scale: 0.08 interlayer_alpha: 0.4 diff --git a/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h b/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h index f08a1eb..ea5cd32 100644 --- a/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h +++ b/hydra_visualizer/include/hydra_visualizer/adapters/edge_color.h @@ -126,6 +126,30 @@ struct ValueEdgeColorAdapter : EdgeColorAdapter { void declare_config(ValueEdgeColorAdapter::Config& config); +struct TraversabilityEdgeColorAdapter : EdgeColorAdapter { + struct Config { + visualizer::RangeColormap::Config colormap{ + config::VirtualConfig{ + visualizer::QualityPalette::Config()}}; + spark_dsg::Color active_color = spark_dsg::Color::pink(); + spark_dsg::Color backend_color = spark_dsg::Color::blue(); + } const config; + + explicit TraversabilityEdgeColorAdapter(const Config& config); + void setGraph(const spark_dsg::DynamicSceneGraph& graph, + spark_dsg::LayerId layer) override; + EdgeColor getColor(const spark_dsg::DynamicSceneGraph& graph, + const spark_dsg::SceneGraphEdge& edge) const override; + + private: + double min_value_; + double max_value_; + const visualizer::RangeColormap colormap_; + REGISTER_COLOR_ADAPTER(TraversabilityEdgeColorAdapter); +}; + +void declare_config(TraversabilityEdgeColorAdapter::Config& config); + #undef REGISTER_COLOR_ADAPTER } // namespace hydra diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h index 74c0736..16d4c58 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/traversability_plugin.h @@ -58,7 +58,7 @@ class TraversabilityPlugin : public VisualizerPlugin { float slice_height = 2.0f; //! line width of the boundary markers - float line_width = 0.05f; + float line_width = 0.07f; }; TraversabilityPlugin(const Config& config, @@ -82,6 +82,14 @@ class TraversabilityPlugin : public VisualizerPlugin { const spark_dsg::SceneGraphLayer& layer, visualization_msgs::msg::MarkerArray& msg) const; + void drawBlockBoundary(const Config& config, + const spark_dsg::TraversabilityNodeAttributes& attrs, + visualization_msgs::msg::Marker& marker) const; + + void drawRegionBoundary(const Config& config, + const spark_dsg::TravNodeAttributes& attrs, + visualization_msgs::msg::Marker& marker) const; + void addBoundaryPoint(const Config& config, visualization_msgs::msg::Marker& marker, const Eigen::Vector3d& point, @@ -91,6 +99,14 @@ class TraversabilityPlugin : public VisualizerPlugin { config::DynamicConfig config_; rclcpp::Publisher::SharedPtr pub_; mutable MarkerTracker tracker_; + + // Start and stop indices for the corner points of the boundary along the state + // direction. + inline static const std::array, 4> state_pairs_ = { + std::make_pair(1, 0), + std::make_pair(1, 2), + std::make_pair(2, 3), + std::make_pair(0, 3)}; }; void declare_config(TraversabilityPlugin::Config& config); diff --git a/hydra_visualizer/launch/static_visualizer.launch.yaml b/hydra_visualizer/launch/static_visualizer.launch.yaml index 7d25e0f..e15d0a3 100644 --- a/hydra_visualizer/launch/static_visualizer.launch.yaml +++ b/hydra_visualizer/launch/static_visualizer.launch.yaml @@ -8,7 +8,6 @@ launch: - 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} # visualizer node and control for launching - node: pkg: hydra_visualizer @@ -22,7 +21,6 @@ launch: --config-utilities-file $(var external_plugins_path) --config-utilities-yaml {glog_level: 0, glog_verbosity: $(var verbosity)} --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} - arg: {name: rviz_path, default: $(find-pkg-share hydra_visualizer)/rviz/static_visualizer.rviz, description: rviz file to load} diff --git a/hydra_visualizer/launch/streaming_visualizer.launch.yaml b/hydra_visualizer/launch/streaming_visualizer.launch.yaml index b9acaa2..b4ff588 100644 --- a/hydra_visualizer/launch/streaming_visualizer.launch.yaml +++ b/hydra_visualizer/launch/streaming_visualizer.launch.yaml @@ -8,7 +8,6 @@ launch: - 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} @@ -28,7 +27,6 @@ launch: --config-utilities-file $(var external_plugins_path) --config-utilities-yaml {glog_level: 1, glog_verbosity: $(var verbosity)} --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} - arg: {name: rviz_path, default: $(find-pkg-share hydra_visualizer)/rviz/streaming_visualizer.rviz, description: rviz file to load} diff --git a/hydra_visualizer/src/adapters/edge_color.cpp b/hydra_visualizer/src/adapters/edge_color.cpp index 631fcf7..01e7100 100644 --- a/hydra_visualizer/src/adapters/edge_color.cpp +++ b/hydra_visualizer/src/adapters/edge_color.cpp @@ -115,4 +115,53 @@ void declare_config(ValueEdgeColorAdapter::Config& config) { field(config.value_functor, "value_functor"); } +TraversabilityEdgeColorAdapter::TraversabilityEdgeColorAdapter(const Config& config) + : config(config), min_value_(0.0), max_value_(1.0), colormap_(config.colormap) {} + +void TraversabilityEdgeColorAdapter::setGraph(const DynamicSceneGraph& graph, + LayerId layer) { + if (!graph.hasLayer(layer)) { + return; + } + bool is_first = true; + for (const auto& [key, edge] : graph.getLayer(layer).edges()) { + const auto value = edge.attributes().weight; + if (value < 0.0) { + continue; + } + if (is_first) { + min_value_ = value; + max_value_ = value; + is_first = false; + } else { + min_value_ = std::min(value, min_value_); + max_value_ = std::max(value, max_value_); + } + } +} + +EdgeColor TraversabilityEdgeColorAdapter::getColor(const DynamicSceneGraph&, + const SceneGraphEdge& edge) const { + const double weight = edge.attributes().weight; + if (weight == -1.0) { + return {config.active_color, config.active_color}; + } + if (weight == -2.0) { + return {config.backend_color, config.backend_color}; + } + if (weight < 0.0) { + return {Color::gray(), Color::gray()}; + } + const auto color = colormap_.getColor(weight, min_value_, max_value_); + return {color, color}; +} + +void declare_config(TraversabilityEdgeColorAdapter::Config& config) { + using namespace config; + name("TraversabilityEdgeColorAdapter::Config"); + field(config.colormap, "colormap"); + field(config.active_color, "active_color"); + field(config.backend_color, "backend_color"); +} + } // namespace hydra diff --git a/hydra_visualizer/src/plugins/traversability_plugin.cpp b/hydra_visualizer/src/plugins/traversability_plugin.cpp index 9cd5c52..4353b17 100644 --- a/hydra_visualizer/src/plugins/traversability_plugin.cpp +++ b/hydra_visualizer/src/plugins/traversability_plugin.cpp @@ -63,6 +63,7 @@ using spark_dsg::SceneGraphLayer; using spark_dsg::SceneGraphNode; using spark_dsg::TraversabilityNodeAttributes; using spark_dsg::TraversabilityState; +using spark_dsg::TravNodeAttributes; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -128,74 +129,96 @@ void TraversabilityPlugin::drawBoundaries(const Config& config, marker.ns = "boundaries"; marker.pose.orientation.w = 1.0; marker.scale.x = config.line_width; - - // Start and stop indices for the corner points of the boundary along the state - // direction. - static const std::array, 4> state_pairs = { - std::make_pair(1, 0), - std::make_pair(1, 2), - std::make_pair(2, 3), - std::make_pair(0, 3)}; - + marker.scale.y = config.line_width; + marker.scale.z = config.line_width; for (const auto& [node_id, node] : layer.nodes()) { - const auto& attrs = node->attributes(); - // Reset the marker. marker.id = id++; marker.points.clear(); marker.colors.clear(); - // Get the world frame positions of the boundary points, adjusted for the line width - // for non-overlapping rendering. bot-right, bot-left, top-left, top-right - std::vector pts; - pts.reserve(4); - pts.emplace_back(attrs.boundary.max.x() - config.line_width, - attrs.boundary.min.y() + config.line_width, - 0); - pts.emplace_back(attrs.boundary.min.x() + config.line_width, - attrs.boundary.min.y() + config.line_width, - 0); - pts.emplace_back(attrs.boundary.min.x() + config.line_width, - attrs.boundary.max.y() - config.line_width, - 0); - pts.emplace_back(attrs.boundary.max.x() - config.line_width, - attrs.boundary.max.y() - config.line_width, - 0); - for (auto& point : pts) { - point += attrs.position; - point.z() = config.slice_height; + auto block_attrs = node->tryAttributes(); + if (block_attrs) { + drawBlockBoundary(config, *block_attrs, marker); + } else { + auto region_attrs = node->tryAttributes(); + if (region_attrs) { + drawRegionBoundary(config, *region_attrs, marker); + } } - // Draw the boundary points as a line segment, where individual states break up the - // line in equal parts if present. - for (size_t i = 0; i < 4; ++i) { - const auto& states = attrs.boundary.states[i]; - const size_t start = state_pairs[i].first; - const size_t end = state_pairs[i].second; - tf2::convert(pts[start], marker.points.emplace_back()); // First point. - - if (states.empty()) { - // Single unknown boundary. - addBoundaryPoint(config, marker, pts[end], TraversabilityState::UNKNOWN, true); - continue; - } + tracker_.add(marker, msg); + } +} - // Add line segments for continuous states. - auto current_state = states[0]; - for (size_t j = 1; j < states.size(); ++j) { - if (states[j] != current_state) { - const double fraction = static_cast(j) / (states.size() - 1); - Eigen::Vector3d segment_point = - pts[start] * (1.0 - fraction) + pts[end] * fraction; - addBoundaryPoint(config, marker, segment_point, current_state); - current_state = states[j]; - } +void TraversabilityPlugin::drawBlockBoundary( + const Config& config, + const spark_dsg::TraversabilityNodeAttributes& attrs, + visualization_msgs::msg::Marker& marker) const { + // Get the world frame positions of the boundary points, adjusted for the line width + // for non-overlapping rendering. bot-right, bot-left, top-left, top-right + std::vector pts; + pts.reserve(4); + pts.emplace_back(attrs.boundary.max.x() - config.line_width, + attrs.boundary.min.y() + config.line_width, + 0); + pts.emplace_back(attrs.boundary.min.x() + config.line_width, + attrs.boundary.min.y() + config.line_width, + 0); + pts.emplace_back(attrs.boundary.min.x() + config.line_width, + attrs.boundary.max.y() - config.line_width, + 0); + pts.emplace_back(attrs.boundary.max.x() - config.line_width, + attrs.boundary.max.y() - config.line_width, + 0); + for (auto& point : pts) { + point += attrs.position; + point.z() = config.slice_height; + } + + // Draw the boundary points as a line segment, where individual states break up the + // line in equal parts if present. + for (size_t i = 0; i < 4; ++i) { + const auto& states = attrs.boundary.states[i]; + const size_t start = state_pairs_[i].first; + const size_t end = state_pairs_[i].second; + tf2::convert(pts[start], marker.points.emplace_back()); // First point. + + if (states.empty()) { + // Single unknown boundary. + addBoundaryPoint(config, marker, pts[end], TraversabilityState::UNKNOWN, true); + continue; + } + + // Add line segments for continuous states. + auto current_state = states[0]; + for (size_t j = 1; j < states.size(); ++j) { + if (states[j] != current_state) { + const double fraction = static_cast(j) / (states.size() - 1); + Eigen::Vector3d segment_point = + pts[start] * (1.0 - fraction) + pts[end] * fraction; + addBoundaryPoint(config, marker, segment_point, current_state); + current_state = states[j]; } - addBoundaryPoint(config, marker, pts[end], current_state, true); } + addBoundaryPoint(config, marker, pts[end], current_state, true); + } +} - // Wrap around the last point to the first point. - tracker_.add(marker, msg); +void TraversabilityPlugin::drawRegionBoundary( + const Config& config, + const TravNodeAttributes& attrs, + visualization_msgs::msg::Marker& marker) const { + marker.type = Marker::LINE_STRIP; + for (size_t i = 0; i < attrs.radii.size(); ++i) { + tf2::convert(attrs.getBoundaryPoint(i), marker.points.emplace_back()); + marker.colors.emplace_back( + visualizer::makeColorMsg(config.colors[static_cast(attrs.states[i])])); + } + // Close the circle. + if (!attrs.radii.empty()) { + marker.points.emplace_back(marker.points.front()); + marker.colors.emplace_back(marker.colors.front()); } }