From df671df547f335da31621645affba9b2e8b33532 Mon Sep 17 00:00:00 2001 From: ktro2828 Date: Tue, 20 Aug 2024 00:56:35 +0900 Subject: [PATCH] feat: add support of `autoware_map_msgs::msg::LaneletMapBin` Signed-off-by: ktro2828 --- awviz_plugin/CMakeLists.txt | 3 + .../autoware_map/lanelet_display.hpp | 110 +++++++++++++ awviz_plugin/package.xml | 5 +- awviz_plugin/plugin_description.xml | 7 + .../src/autoware_map/lanelet_display.cpp | 147 ++++++++++++++++++ 5 files changed, 271 insertions(+), 1 deletion(-) create mode 100644 awviz_plugin/include/awviz_plugin/autoware_map/lanelet_display.hpp create mode 100644 awviz_plugin/src/autoware_map/lanelet_display.cpp diff --git a/awviz_plugin/CMakeLists.txt b/awviz_plugin/CMakeLists.txt index b0dd4f2..4e76153 100644 --- a/awviz_plugin/CMakeLists.txt +++ b/awviz_plugin/CMakeLists.txt @@ -37,6 +37,9 @@ ament_target_dependencies( rclcpp cv_bridge sensor_msgs + lanelet2_core + autoware_lanelet2_extension + autoware_map_msgs autoware_perception_msgs ) diff --git a/awviz_plugin/include/awviz_plugin/autoware_map/lanelet_display.hpp b/awviz_plugin/include/awviz_plugin/autoware_map/lanelet_display.hpp new file mode 100644 index 0000000..27dd375 --- /dev/null +++ b/awviz_plugin/include/awviz_plugin/autoware_map/lanelet_display.hpp @@ -0,0 +1,110 @@ +// Copyright 2024 Kotaro Uetake. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWVIZ_PLUGIN__AUTOWARE_MAP__LANELET_DISPLAY_HPP_ +#define AWVIZ_PLUGIN__AUTOWARE_MAP__LANELET_DISPLAY_HPP_ + +#include +#include + +#include + +#include + +#include +#include + +namespace awviz_plugin +{ +/** + * @brief A class stores a set of vertices and normals, and creates a new `rerun::Mesh3D` object. + */ +class LaneletMesh +{ +public: + LaneletMesh() = default; + + /** + * @brief Append new vertex and normal both to the end of the container. + * + * @param vertex Vertex element. + * @param normal Vertex normal. + */ + void emplace_back(const rerun::Position3D & vertex, const rerun::Vector3D & normal) + { + vertices_.emplace_back(vertex); + normals_.emplace_back(normal); + } + + /** + * @brief Append a new vertex to the end of the container. Then, the normal is `{0.0f, + * 0.0f, 1.0f}`. + * + * @param vertex Vertex element. + */ + void emplace_back(const rerun::Position3D & vertex) + { + vertices_.emplace_back(vertex); + normals_.emplace_back(0.f, 0.f, 1.0f); + } + + /** + * @brief Create a `rerun::Mesh3D` object with specifying normals. + * @todo `rerun::Mesh3D` supports `::with_albedo_factor()` from `v0.18.0`. + * + * @return New `rerun::Mesh3D` object. + */ + rerun::Mesh3D as_mesh() const { return rerun::Mesh3D(vertices_).with_vertex_normals(normals_); } + +private: + std::vector vertices_; //!< Set of positions of each vertex. + std::vector normals_; //!< Set of normals of each vertex. +}; + +/** + * @brief A display plugin for `autoware_map_msgs::msg::LaneletMapBin`. + */ +class LaneletDisplay : public awviz_common::RosTopicDisplay +{ +public: + /** + * @brief Construct a new object. + */ + LaneletDisplay(); + +protected: + void log_message(typename autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) override; + +private: + /** + * @brief Convert lanelets of roads to a single triangle mesh. + * + * @param lanelets Set of all lanelets associated with any roads. + */ + LaneletMesh convert_road_lanelets(const lanelet::ConstLanelets & road_lanelets) const; + + /** + * @brief Convert crosswalks to a set of triangle mesh. + * + * @param all_lanelets Set of all lanelets associated with the vector map. + * @param vertices Output container to store vertices. + */ + std::vector convert_crosswalks(const lanelet::ConstLanelets & all_lanelets) const; + +private: + std::shared_ptr lanelet_map_ = + std::make_shared(); //!< Shared pointer stores `lanelet::LaneletMap`. +}; +} // namespace awviz_plugin +#endif // AWVIZ_PLUGIN__AUTOWARE_MAP__LANELET_DISPLAY_HPP_ diff --git a/awviz_plugin/package.xml b/awviz_plugin/package.xml index dcbdc23..95e0ce1 100644 --- a/awviz_plugin/package.xml +++ b/awviz_plugin/package.xml @@ -14,7 +14,10 @@ pluginlib sensor_msgs rclcpp + autoware_map_msgs autoware_perception_msgs + lanelet2_core + autoware_lanelet2_extension ament_lint_auto ament_lint_common @@ -22,4 +25,4 @@ ament_cmake - + \ No newline at end of file diff --git a/awviz_plugin/plugin_description.xml b/awviz_plugin/plugin_description.xml index fa8670d..a8f86a4 100644 --- a/awviz_plugin/plugin_description.xml +++ b/awviz_plugin/plugin_description.xml @@ -31,6 +31,13 @@ + + + Display from autoware_map_msgs/msg/LaneletMapBin message. + autoware_map_msgs/msg/LaneletMapBin + + diff --git a/awviz_plugin/src/autoware_map/lanelet_display.cpp b/awviz_plugin/src/autoware_map/lanelet_display.cpp new file mode 100644 index 0000000..0269087 --- /dev/null +++ b/awviz_plugin/src/autoware_map/lanelet_display.cpp @@ -0,0 +1,147 @@ +// Copyright 2024 Kotaro Uetake. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awviz_plugin/autoware_map/lanelet_display.hpp" + +#include +#include + +#include + +namespace +{ +/** + * @brief Check if the input linestring has same attribute in the specfied set of attributes. + * + * @param linestring LineString object. + * @param attributes Set of attributes. + * @return Return true if the linestring has. + */ +bool has_attribute( + const lanelet::ConstLineString3d & linestring, const std::set & attributes) +{ + if (!linestring.hasAttribute(lanelet::AttributeName::Type)) { + return false; + } else { + const auto & attr = linestring.attribute(lanelet::AttributeName::Type); + return attributes.count(attr.value()) > 0; + } +} +} // namespace + +namespace awviz_plugin +{ +LaneletDisplay::LaneletDisplay() +: awviz_common::RosTopicDisplay() +{ +} + +void LaneletDisplay::log_message(autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) +{ + stream_->set_time_seconds( + TIMELINE_NAME, rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec).seconds()); + + const auto entity_path = property_.entity(msg->header.frame_id); + if (!entity_path) { + stream_->log(property_.topic(), rerun::TextLog("There is no corresponding entity path")); + return; + } + + // Load LaneletMap + lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_); + + const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); + + std::vector all_meshes; + { + // 1. all road lanelets to a single mesh + const auto road_mesh = convert_road_lanelets(all_lanelets); + all_meshes.emplace_back(road_mesh.as_mesh()); + } + + { + // 1. all crosswalk lanelets to a set of mesh + const auto crosswalk_meshes = convert_crosswalks(all_lanelets); + for (const auto & mesh : crosswalk_meshes) { + all_meshes.emplace_back(mesh.as_mesh()); + } + } + + stream_->log(entity_path.value(), all_meshes); +} + +LaneletMesh LaneletDisplay::convert_road_lanelets(const lanelet::ConstLanelets & all_lanelets) const +{ + static const std::set attributes{"line_thin", "line_thick"}; + + LaneletMesh output; + + const auto road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + for (const auto & lanelet : road_lanelets) { + const auto left_bound = lanelet.leftBound(); + const auto right_bound = lanelet.rightBound(); + + const auto left_candidates = lanelet_map_->laneletLayer.findUsages(left_bound); + for (const auto & candidate : left_candidates) { + if (candidate == lanelet) { + continue; // exclude self lanelet + } else if (candidate.leftBound() != right_bound && candidate.rightBound() != left_bound) { + continue; // exclude unshared lanelet + } + + if (has_attribute(left_bound, attributes)) { + for (const auto & bound : left_bound) { + output.emplace_back(rerun::Position3D(bound.x(), bound.y(), bound.z())); + } + } + } + + const auto right_candidates = lanelet_map_->laneletLayer.findUsages(right_bound); + for (const auto & candidate : right_candidates) { + if (candidate == lanelet) { + continue; // exclude self lanelet + } else if (candidate.leftBound() != right_bound && candidate.rightBound() != left_bound) { + continue; // exclude unshared lanelet + } + + if (has_attribute(right_bound, attributes)) { + for (const auto & bound : right_bound) { + output.emplace_back(rerun::Position3D(bound.x(), bound.y(), bound.z())); + } + } + } + } + return output; +} + +std::vector LaneletDisplay::convert_crosswalks( + const lanelet::ConstLanelets & all_lanelets) const +{ + std::vector output; + + const auto crosswalks = lanelet::utils::query::crosswalkLanelets(all_lanelets); + for (const auto & lanelet : crosswalks) { + LaneletMesh current; + for (const auto & point : lanelet.polygon3d()) { + current.emplace_back(rerun::Position3D(point.x(), point.y(), point.z())); + } + output.emplace_back(current); + } + + return output; +} +} // namespace awviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(awviz_plugin::LaneletDisplay, awviz_common::Display);