9 #include <Ice/Config.h>
11 #include <SimoxUtility/color/cmaps/colormaps.h>
16 #include <RobotAPI/interface/ArViz/Elements.h>
27 const std::string& name,
30 const auto cmap = simox::color::cmaps::viridis();
31 const float vmax = costmap.
getGrid().array().maxCoeff();
33 const auto asColor = [&cmap, &vmax](
const float distance,
36 const auto color = cmap.at(
distance, 0.
F, vmax);
39 const Ice::Byte alpha = isValid &&
distance >= 0 ? color.a : 0;
40 return {alpha, color.r, color.g, color.b};
43 const std::int64_t cols = costmap.
getGrid().cols();
44 const std::int64_t rows = costmap.
getGrid().rows();
48 std::vector<std::vector<Eigen::Vector3f>> vertices;
49 std::vector<std::vector<viz::data::Color>> colors;
51 for (
int r = 0; r < rows; r++)
53 auto& verticesRow = vertices.emplace_back(cols);
54 auto& colorsRow = colors.emplace_back(cols);
56 for (
int c = 0;
c < cols;
c++)
62 colorsRow.at(
c) = asColor(costmap.
getGrid()(r,
c), isValid);
66 mesh.grid2D(vertices, colors);
67 mesh.position({0, 0, zOffset});