Merge pull request tier4#1597 from tier4/feature/render-omitted-light-bulb

Author: HansRobo

simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp

@@ -202,8 +202,9 @@ class HdMapUtils
   auto getTangentVector(const lanelet::Id, const double s) const
     -> std::optional<geometry_msgs::msg::Vector3>;
 
-  auto getTrafficLightBulbPosition(const lanelet::Id traffic_light_id, const std::string &) const
-    -> std::optional<geometry_msgs::msg::Point>;
+  auto getTrafficLightBulbPosition(
+    const lanelet::Id traffic_light_id, const std::string &,
+    const bool allow_infer_position = false) const -> std::optional<geometry_msgs::msg::Point>;
 
   auto getTrafficLightIds() const -> lanelet::Ids;
 

simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp

@@ -438,7 +438,7 @@ struct TrafficLight
 
   std::set<Bulb> bulbs;
 
-  const std::map<Bulb::Hash, std::optional<geometry_msgs::msg::Point>> positions;
+  const std::map<Color, std::optional<geometry_msgs::msg::Point>> positions;
 
   auto clear() { bulbs.clear(); }
 
@@ -454,31 +454,45 @@ struct TrafficLight
   template <typename Markers, typename Now>
   auto draw(Markers & markers, const Now & now, const std::string & frame_id) const
   {
-    auto optional_position = [this](auto && bulb) {
-      try {
-        return positions.at(bulb.hash() & 0b1111'0000'1111'1111);  // NOTE: Ignore status
-      } catch (const std::out_of_range &) {
-        return std::optional<geometry_msgs::msg::Point>(std::nullopt);
-      }
+    auto create_bulb_marker = [&](const Color & color, const bool is_on) {
+      visualization_msgs::msg::Marker marker;
+      marker.header.stamp = now;
+      marker.header.frame_id = frame_id;
+      marker.action = visualization_msgs::msg::Marker::ADD;
+      marker.ns = "bulb";
+      marker.id = way_id << 2 | static_cast<int>(color.value);
+      marker.type = visualization_msgs::msg::Marker::SPHERE;
+      marker.pose.position = positions.at(color).value();
+      marker.pose.orientation = geometry_msgs::msg::Quaternion();
+      marker.scale.x = 0.3;
+      marker.scale.y = 0.3;
+      marker.scale.z = 0.3;
+      marker.color = color_names::makeColorMsg(boost::lexical_cast<std::string>(color));
+      marker.color.a = is_on ? 1.0 : 0.3;
+      return marker;
     };
 
+    std::set<Color> added_colors;
+
+    // Place on markers for actual bulbs
     for (const auto & bulb : bulbs) {
-      if (optional_position(bulb).has_value() and bulb.is(Shape::Category::circle)) {
-        visualization_msgs::msg::Marker marker;
-        marker.header.stamp = now;
-        marker.header.frame_id = frame_id;
-        marker.action = marker.ADD;
-        marker.ns = "bulb";
-        marker.id = way_id;
-        marker.type = marker.SPHERE;
-        marker.pose.position = optional_position(bulb).value();
-        marker.pose.orientation = geometry_msgs::msg::Quaternion();
-        marker.scale.x = 0.3;
-        marker.scale.y = 0.3;
-        marker.scale.z = 0.3;
-        marker.color =
-          color_names::makeColorMsg(boost::lexical_cast<std::string>(std::get<Color>(bulb.value)));
-        markers.push_back(marker);
+      // NOTE: Status is ignored intentionally
+      if (bulb.is(Shape::Category::circle)) {
+        const auto color = std::get<Color>(bulb.value);
+        const auto position = positions.find(color);
+        if (position == positions.end() or not position->second.has_value()) {
+          continue;
+        }
+
+        markers.push_back(create_bulb_marker(color, true));
+        added_colors.insert(color);
+      }
+    }
+
+    // Place solidOff markers for other positions
+    for (const auto & [color, position] : positions) {
+      if (position.has_value() and added_colors.find(color) == added_colors.end()) {
+        markers.push_back(create_bulb_marker(color, false));
       }
     }
   }

simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp

@@ -45,7 +45,7 @@ class ConventionalTrafficLights : public TrafficLightsBase
   ~ConventionalTrafficLights() override = default;
 
 private:
-  auto update() const -> void
+  auto update() const -> void override
   {
     backward_compatible_publisher_ptr_->publish(
       clock_ptr_->now(), generateUpdateTrafficLightsRequest());

simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

@@ -705,8 +705,8 @@ auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids
 }
 
 auto HdMapUtils::getTrafficLightBulbPosition(
-  const lanelet::Id traffic_light_id, const std::string & color_name) const
-  -> std::optional<geometry_msgs::msg::Point>
+  const lanelet::Id traffic_light_id, const std::string & color_name,
+  const bool allow_infer_position) const -> std::optional<geometry_msgs::msg::Point>
 {
   lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_);
   auto autoware_traffic_lights = lanelet::utils::query::autowareTrafficLights(all_lanelets);
@@ -737,6 +737,39 @@ auto HdMapUtils::getTrafficLightBulbPosition(
       }
     }
   }
+
+  if (!allow_infer_position) {
+    return std::nullopt;
+  }
+
+  // In case of a traffic light without bulbs, we can check the base string
+  // to get the position of the traffic light
+  const auto position_scale = color_name == "red" ? 0.25 : (color_name == "yellow" ? 0.5 : 0.75);
+  for (const auto & light : autoware_traffic_lights) {
+    for (auto & traffic_light : light->trafficLights()) {
+      if (traffic_light.id() != traffic_light_id) continue;
+      if (!traffic_light.isLineString()) continue;
+      const auto base_string = static_cast<lanelet::ConstLineString3d>(traffic_light);
+      if (!base_string.hasAttribute("height")) continue;
+      const auto height = base_string.attribute("height").asDouble();
+      if (!height) continue;
+
+      const auto x1 = base_string.front().x();
+      const auto y1 = base_string.front().y();
+      const auto z1 = base_string.front().z();
+
+      const auto x2 = base_string.back().x();
+      const auto y2 = base_string.back().y();
+
+      geometry_msgs::msg::Point point;
+      point.x = x1 * position_scale + x2 * (1 - position_scale);
+      point.y = y1 * position_scale + y2 * (1 - position_scale);
+      point.z = z1 + *height / 2.0;
+
+      return point;
+    }
+  }
+
   return std::nullopt;
 }
 

simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp

@@ -181,15 +181,9 @@ TrafficLight::TrafficLight(
   }()),
   regulatory_elements_ids(hdmap_utils.getTrafficLightRegulatoryElementIDsFromTrafficLight(way_id)),
   positions{
-    std::make_pair(
-      Bulb(Color::green, Status::solid_on, Shape::circle).hash(),
-      hdmap_utils.getTrafficLightBulbPosition(way_id, "green")),
-    std::make_pair(
-      Bulb(Color::yellow, Status::solid_on, Shape::circle).hash(),
-      hdmap_utils.getTrafficLightBulbPosition(way_id, "yellow")),
-    std::make_pair(
-      Bulb(Color::red, Status::solid_on, Shape::circle).hash(),
-      hdmap_utils.getTrafficLightBulbPosition(way_id, "red")),
+    std::make_pair(Color::green, hdmap_utils.getTrafficLightBulbPosition(way_id, "green", true)),
+    std::make_pair(Color::yellow, hdmap_utils.getTrafficLightBulbPosition(way_id, "yellow", true)),
+    std::make_pair(Color::red, hdmap_utils.getTrafficLightBulbPosition(way_id, "red", true)),
   }
 {
 }