feat: add support of autoware_map_msgs::msg::LaneletMapBin

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

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
 )
 

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 <awviz_common/display.hpp>
+#include <rerun.hpp>
+
+#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+
+#include <memory>
+#include <vector>
+
+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<rerun::Position3D> vertices_;  //!< Set of positions of each vertex.
+  std::vector<rerun::Vector3D> normals_;     //!< Set of normals of each vertex.
+};
+
+/**
+ * @brief A display plugin for `autoware_map_msgs::msg::LaneletMapBin`.
+ */
+class LaneletDisplay : public awviz_common::RosTopicDisplay<autoware_map_msgs::msg::LaneletMapBin>
+{
+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<LaneletMesh> convert_crosswalks(const lanelet::ConstLanelets & all_lanelets) const;
+
+private:
+  std::shared_ptr<lanelet::LaneletMap> lanelet_map_ =
+    std::make_shared<lanelet::LaneletMap>();  //!< Shared pointer stores `lanelet::LaneletMap`.
+};
+}  // namespace awviz_plugin
+#endif  // AWVIZ_PLUGIN__AUTOWARE_MAP__LANELET_DISPLAY_HPP_

awviz_plugin/package.xml

@@ -14,12 +14,15 @@
   <depend>pluginlib</depend>
   <depend>sensor_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>autoware_map_msgs</depend>
   <depend>autoware_perception_msgs</depend>
+  <depend>lanelet2_core</depend>
+  <depend>autoware_lanelet2_extension</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

awviz_plugin/plugin_description.xml

@@ -31,6 +31,13 @@
     </class>
 
     <!-- Autoware -->
+    <!-- Map -->
+    <class name="awviz_plugin/LaneletDisplay" type="awviz_plugin::LaneletDisplay"
+        base_class_type="awviz_common::Display">
+        <description>Display from autoware_map_msgs/msg/LaneletMapBin message.</description>
+        <message_type>autoware_map_msgs/msg/LaneletMapBin</message_type>
+    </class>
+
     <!-- Perception -->
     <class name="awviz_plugin/DetectedObjectsDisplay" type="awviz_plugin::DetectedObjectsDisplay"
         base_class_type="awviz_common::Display">

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 <autoware_lanelet2_extension/utility/message_conversion.hpp>
+#include <autoware_lanelet2_extension/utility/query.hpp>
+
+#include <set>
+
+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<std::string> & 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<autoware_map_msgs::msg::LaneletMapBin>()
+{
+}
+
+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<rerun::Mesh3D> 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<std::string> 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<LaneletMesh> LaneletDisplay::convert_crosswalks(
+  const lanelet::ConstLanelets & all_lanelets) const
+{
+  std::vector<LaneletMesh> 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/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(awviz_plugin::LaneletDisplay, awviz_common::Display);