Skip to content

Commit 2f457df

Browse files
nathanhhughesnathanhhughes
authored
Feature/layer boundary plugin (#63)
* add plugin to add boundaries for nodes without extra topic * get wrap logic correct
1 parent c979c45 commit 2f457df

3 files changed

Lines changed: 198 additions & 0 deletions

File tree

‎hydra_visualizer/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@ add_library(
4848
src/plugins/mesh_point_plugin.cpp
4949
src/plugins/places_freespace_plugin.cpp
5050
src/plugins/pose_plugin.cpp
51+
src/plugins/region_growing_boundary_plugin.cpp
5152
src/plugins/traversability_plugin.cpp
5253
src/scene_graph_renderer.cpp
5354
src/utils/ear_clipping.cpp

‎hydra_visualizer/include/hydra_visualizer/plugins/region_growing_boundary_plugin.h‎

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
/* -----------------------------------------------------------------------------
2+
* Copyright 2022 Massachusetts Institute of Technology.
3+
* All Rights Reserved
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions are met:
7+
*
8+
* 1. Redistributions of source code must retain the above copyright notice,
9+
* this list of conditions and the following disclaimer.
10+
*
11+
* 2. Redistributions in binary form must reproduce the above copyright notice,
12+
* this list of conditions and the following disclaimer in the documentation
13+
* and/or other materials provided with the distribution.
14+
*
15+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16+
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17+
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18+
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
19+
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20+
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
21+
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
22+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
23+
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24+
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25+
*
26+
* Research was sponsored by the United States Air Force Research Laboratory and
27+
* the United States Air Force Artificial Intelligence Accelerator and was
28+
* accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views
29+
* and conclusions contained in this document are those of the authors and should
30+
* not be interpreted as representing the official policies, either expressed or
31+
* implied, of the United States Air Force or the U.S. Government. The U.S.
32+
* Government is authorized to reproduce and distribute reprints for Government
33+
* purposes notwithstanding any copyright notation herein.
34+
* -------------------------------------------------------------------------- */
35+
#pragma once
36+
37+
#include <config_utilities/dynamic_config.h>
38+
39+
#include "hydra_visualizer/plugins/layer_plugin.h"
40+
41+
namespace hydra {
42+
43+
class RegionGrowingBoundaryPlugin : public LayerPlugin {
44+
public:
45+
struct Config {
46+
//! line width of the boundary markers
47+
float line_width = 0.07f;
48+
//! Colors representing each traversability state.
49+
std::vector<spark_dsg::Color> colors{spark_dsg::Color::gray(), // Unknown
50+
spark_dsg::Color::blue(), // Traversable
51+
spark_dsg::Color::red(), // Intraversable
52+
spark_dsg::Color::green()}; // Traversed
53+
};
54+
55+
RegionGrowingBoundaryPlugin(const Config& config, const std::string& ns);
56+
57+
virtual ~RegionGrowingBoundaryPlugin() = default;
58+
59+
virtual void draw(const std_msgs::msg::Header& header,
60+
const visualizer::LayerInfo& info,
61+
const spark_dsg::SceneGraphLayer& layer,
62+
const spark_dsg::Mesh* mesh,
63+
visualization_msgs::msg::MarkerArray& msg,
64+
MarkerTracker& tracker) override;
65+
66+
protected:
67+
const std::string ns_;
68+
config::DynamicConfig<Config> config_;
69+
};
70+
71+
void declare_config(RegionGrowingBoundaryPlugin::Config& config);
72+
73+
} // namespace hydra

‎hydra_visualizer/src/plugins/region_growing_boundary_plugin.cpp‎

Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
/* -----------------------------------------------------------------------------
2+
* Copyright 2022 Massachusetts Institute of Technology.
3+
* All Rights Reserved
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions are met:
7+
*
8+
* 1. Redistributions of source code must retain the above copyright notice,
9+
* this list of conditions and the following disclaimer.
10+
*
11+
* 2. Redistributions in binary form must reproduce the above copyright notice,
12+
* this list of conditions and the following disclaimer in the documentation
13+
* and/or other materials provided with the distribution.
14+
*
15+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16+
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17+
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18+
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
19+
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20+
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
21+
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
22+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
23+
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24+
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25+
*
26+
* Research was sponsored by the United States Air Force Research Laboratory and
27+
* the United States Air Force Artificial Intelligence Accelerator and was
28+
* accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views
29+
* and conclusions contained in this document are those of the authors and should
30+
* not be interpreted as representing the official policies, either expressed or
31+
* implied, of the United States Air Force or the U.S. Government. The U.S.
32+
* Government is authorized to reproduce and distribute reprints for Government
33+
* purposes notwithstanding any copyright notation herein.
34+
* -------------------------------------------------------------------------- */
35+
#include "hydra_visualizer/plugins/region_growing_boundary_plugin.h"
36+
37+
#include <config_utilities/config.h>
38+
#include <config_utilities/validation.h>
39+
#include <spark_dsg/node_attributes.h>
40+
41+
#include <tf2_eigen/tf2_eigen.hpp>
42+
43+
#include "hydra_visualizer/color/color_parsing.h"
44+
45+
namespace hydra {
46+
namespace {
47+
48+
static const auto registration_ =
49+
config::RegistrationWithConfig<LayerPlugin,
50+
RegionGrowingBoundaryPlugin,
51+
RegionGrowingBoundaryPlugin::Config,
52+
std::string>("RegionGrowingBoundaryPlugin");
53+
54+
std_msgs::msg::ColorRGBA makeBoundaryColor(const std::vector<spark_dsg::Color>& colors,
55+
spark_dsg::TraversabilityState state) {
56+
return visualizer::makeColorMsg(colors.at(static_cast<size_t>(state)));
57+
}
58+
59+
} // namespace
60+
61+
using visualization_msgs::msg::Marker;
62+
63+
void declare_config(RegionGrowingBoundaryPlugin::Config& config) {
64+
using namespace config;
65+
name("RegionGrowingBoundaryPlugin::Config");
66+
field(config.colors, "colors");
67+
field(config.line_width, "line_width");
68+
checkCondition(config.colors.size() == 4, "colors.size() must be 4");
69+
check(config.line_width, GT, 0.0f, "line_width");
70+
}
71+
72+
RegionGrowingBoundaryPlugin::RegionGrowingBoundaryPlugin(const Config& config,
73+
const std::string& ns)
74+
: ns_(ns),
75+
config_(ns + "_mesh_point_plugin", config, [this]() { has_change_ = true; }) {}
76+
77+
void RegionGrowingBoundaryPlugin::draw(const std_msgs::msg::Header& header,
78+
const visualizer::LayerInfo& info,
79+
const spark_dsg::SceneGraphLayer& layer,
80+
const spark_dsg::Mesh*,
81+
visualization_msgs::msg::MarkerArray& msg,
82+
MarkerTracker& tracker) {
83+
const auto config = config_.get();
84+
85+
Marker marker;
86+
marker.id = 0;
87+
marker.header = header;
88+
marker.type = Marker::LINE_LIST;
89+
marker.action = Marker::ADD;
90+
marker.ns = ns_ + "_region_growing_boundaries";
91+
marker.pose.orientation.w = 1.0;
92+
marker.scale.x = config.line_width;
93+
marker.scale.y = config.line_width;
94+
marker.scale.z = config.line_width;
95+
for (const auto& [node_id, node] : layer.nodes()) {
96+
if (info.filter && !info.filter(*node)) {
97+
continue;
98+
}
99+
100+
auto attrs = node->tryAttributes<spark_dsg::TravNodeAttributes>();
101+
if (!attrs) {
102+
continue;
103+
}
104+
105+
for (size_t i = 1; i <= attrs->radii.size(); ++i) {
106+
const auto start_idx = i - 1;
107+
const auto end_idx = i % attrs->radii.size();
108+
auto start = attrs->getBoundaryPoint(start_idx);
109+
start.z() += info.z_offset;
110+
tf2::convert(start, marker.points.emplace_back());
111+
auto end = attrs->getBoundaryPoint(end_idx);
112+
end.z() += info.z_offset;
113+
tf2::convert(end, marker.points.emplace_back());
114+
marker.colors.emplace_back(
115+
makeBoundaryColor(config.colors, attrs->states[start_idx]));
116+
marker.colors.emplace_back(
117+
makeBoundaryColor(config.colors, attrs->states[end_idx]));
118+
}
119+
}
120+
121+
tracker.add(marker, msg);
122+
}
123+
124+
} // namespace hydra

0 commit comments

Comments
 (0)