Merge pull request autowarefoundation#629 from tier4/sync-upstream

chore: sync upstream

common/traffic_light_recognition_marker_publisher/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.14)
+project(traffic_light_recognition_marker_publisher)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/traffic_light_recognition_marker_publisher.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "TrafficLightRecognitionMarkerPublisher"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(INSTALL_TO_SHARE launch)

common/traffic_light_recognition_marker_publisher/Readme.md

@@ -0,0 +1,40 @@
+# path_distance_calculator
+
+## Purpose
+
+This node publishes a marker array for visualizing traffic signal recognition results on Rviz.
+
+![sample_img](./images/traffic_light_recognition_visualization_sample.png)
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                                    | Type                                                     | Description                                       |
+| ------------------------------------------------------- | -------------------------------------------------------- | ------------------------------------------------- |
+| `/map/vector_map`                                       | `autoware_auto_mapping_msgs::msg::HADMapBin`             | Vector map for getting traffic signal information |
+| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | The result of traffic signal recognition          |
+
+### Output
+
+| Name                                                           | Type                                   | Description                                                                    |
+| -------------------------------------------------------------- | -------------------------------------- | ------------------------------------------------------------------------------ |
+| `/perception/traffic_light_recognition/traffic_signals_marker` | `visualization_msgs::msg::MarkerArray` | Publish a marker array for visualization of traffic signal recognition results |
+
+## Parameters
+
+None.
+
+### Node Parameters
+
+None.
+
+### Core Parameters
+
+None.
+
+## Assumptions / Known limits
+
+TBD.

common/traffic_light_recognition_marker_publisher/launch/traffic_light_recognition_marker_publisher.launch.xml

@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="traffic_light_recognition_marker_publisher" exec="traffic_light_recognition_marker_publisher_node" name="traffic_light_recognition_marker_publisher">
+    <remap from="~/input/lanelet2_map" to="/map/vector_map"/>
+    <remap from="~/input/traffic_signals" to="/perception/traffic_light_recognition/traffic_signals"/>
+    <remap from="~/output/marker" to="/perception/traffic_light_recognition/traffic_signals_marker"/>
+  </node>
+</launch>

common/traffic_light_recognition_marker_publisher/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>traffic_light_recognition_marker_publisher</name>
+  <version>0.0.0</version>
+  <description>The traffic_light_recognition_marker_publisher package</description>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="takeshi.miura@tier4.jp">Takeshi Miura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp

@@ -0,0 +1,164 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// 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 "traffic_light_recognition_marker_publisher.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <memory>
+#include <utility>
+#include <vector>
+
+TrafficLightRecognitionMarkerPublisher::TrafficLightRecognitionMarkerPublisher(
+  const rclcpp::NodeOptions & options)
+: Node("traffic_light_recognition_marker_publisher", options)
+{
+  using std::placeholders::_1;
+
+  sub_map_ptr_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
+    "~/input/lanelet2_map", rclcpp::QoS{1}.transient_local(),
+    std::bind(&TrafficLightRecognitionMarkerPublisher::onMap, this, _1));
+  sub_tlr_ptr_ = this->create_subscription<autoware_auto_perception_msgs::msg::TrafficSignalArray>(
+    "~/input/traffic_signals", rclcpp::QoS{1},
+    std::bind(&TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray, this, _1));
+  pub_marker_ptr_ =
+    this->create_publisher<visualization_msgs::msg::MarkerArray>("~/output/marker", rclcpp::QoS{1});
+}
+
+void TrafficLightRecognitionMarkerPublisher::onMap(const HADMapBin::ConstSharedPtr msg_ptr)
+{
+  is_map_ready_ = false;
+  lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap);
+  lanelet::utils::conversion::fromBinMsg(*msg_ptr, viz_lanelet_map);
+  lanelet::ConstLanelets lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map);
+  std::vector<lanelet::AutowareTrafficLightConstPtr> tl_regulatory_elements =
+    lanelet::utils::query::autowareTrafficLights(lanelets);
+  for (const auto & tl : tl_regulatory_elements) {
+    const auto tl_lights = tl->trafficLights();
+    for (const auto & tl_light : tl_lights) {
+      if (tl_light.isLineString()) {
+        lanelet::ConstLineString3d tl_linestring =
+          static_cast<lanelet::ConstLineString3d>(tl_light);
+        int32_t tl_id = static_cast<int32_t>(tl_light.id());
+        Pose tl_pose;
+        tl_pose.position.x = (tl_linestring.front().x() + tl_linestring.back().x()) / 2;
+        tl_pose.position.y = (tl_linestring.front().y() + tl_linestring.back().y()) / 2;
+        tl_pose.position.z = tl_linestring.front().z() + 1.0;
+        tl_position_map_[tl_id] = tl_pose;
+      }
+    }
+  }
+  is_map_ready_ = true;
+}
+
+void TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray(
+  const TrafficSignalArray::ConstSharedPtr msg_ptr)
+{
+  if (!is_map_ready_) {
+    return;
+  }
+  MarkerArray markers;
+  marker_id = 0;
+  for (const auto & tl : msg_ptr->signals) {
+    if (tl_position_map_.count(tl.map_primitive_id) == 0) continue;
+    auto tl_position = tl_position_map_[tl.map_primitive_id];
+    for (const auto tl_light : tl.lights) {
+      const auto marker = getTrafficLightMarker(tl_position, tl_light.color, tl_light.shape);
+      markers.markers.emplace_back(marker);
+      marker_id++;
+      tl_position.position.z += 0.5;
+    }
+  }
+  pub_marker_ptr_->publish(markers);
+}
+
+visualization_msgs::msg::Marker TrafficLightRecognitionMarkerPublisher::getTrafficLightMarker(
+  const Pose & tl_pose, const uint8_t tl_color, const uint8_t tl_shape)
+{
+  visualization_msgs::msg::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = rclcpp::Time();
+  marker.ns = "traffic_light_color";
+  marker.id = marker_id;
+  marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+  marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.pose = tl_pose;
+  marker.color = getTrafficLightColor(tl_color, tl_shape);
+  marker.scale.z = 0.5;
+  marker.frame_locked = false;
+  marker.text = getTrafficLightString(tl_color, tl_shape);
+
+  return marker;
+}
+
+std::string TrafficLightRecognitionMarkerPublisher::getTrafficLightString(
+  const uint8_t tl_color, const uint8_t tl_shape)
+{
+  if (tl_shape == TrafficLight::LEFT_ARROW) return "LEFT_ARROW";
+  if (tl_shape == TrafficLight::RIGHT_ARROW) return "RIGHT_ARROW";
+  if (tl_shape == TrafficLight::UP_ARROW) return "UP_ARROW";
+  if (tl_shape == TrafficLight::DOWN_ARROW) return "DOWN_ARROW";
+  if (tl_shape == TrafficLight::DOWN_LEFT_ARROW) return "DOWN_LEFT_ARROW";
+  if (tl_shape == TrafficLight::DOWN_RIGHT_ARROW) return "DOWN_RIGHT_ARROW";
+
+  if (tl_color == TrafficLight::RED) return "RED";
+  if (tl_color == TrafficLight::GREEN) return "GREEN";
+  if (tl_color == TrafficLight::AMBER) return "AMBER";
+
+  if (tl_shape == TrafficLight::UNKNOWN) return "UNKNOWN";
+
+  return "UNKNOWN";
+}
+
+std_msgs::msg::ColorRGBA TrafficLightRecognitionMarkerPublisher::getTrafficLightColor(
+  const uint8_t tl_color, const uint8_t tl_shape)
+{
+  std_msgs::msg::ColorRGBA c;
+  c.r = 1.0f;
+  c.g = 1.0f;
+  c.b = 1.0f;
+  c.a = 0.999;
+
+  if (
+    tl_shape == TrafficLight::LEFT_ARROW || tl_shape == TrafficLight::RIGHT_ARROW ||
+    tl_shape == TrafficLight::UP_ARROW || tl_shape == TrafficLight::DOWN_ARROW ||
+    tl_shape == TrafficLight::DOWN_LEFT_ARROW || tl_shape == TrafficLight::DOWN_RIGHT_ARROW) {
+    return c;  // white
+  }
+
+  if (tl_color == TrafficLight::RED) {
+    c.r = 1.0f;
+    c.g = 0.0f;
+    c.b = 0.0f;
+    return c;  // red
+  }
+  if (tl_color == TrafficLight::GREEN) {
+    c.r = 0.0f;
+    c.g = 1.0f;
+    c.b = 0.0f;
+    return c;  // green
+  }
+  if (tl_color == TrafficLight::AMBER) {
+    c.r = 1.0f;
+    c.g = 1.0f;
+    c.b = 0.0f;
+    return c;  // amber
+  }
+
+  return c;  // white
+}
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightRecognitionMarkerPublisher)

common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp

@@ -0,0 +1,62 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// 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 TRAFFIC_LIGHT_RECOGNITION_MARKER_PUBLISHER_HPP_
+#define TRAFFIC_LIGHT_RECOGNITION_MARKER_PUBLISHER_HPP_
+
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/visualization/visualization.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <map>
+#include <string>
+
+class TrafficLightRecognitionMarkerPublisher : public rclcpp::Node
+{
+public:
+  using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
+  using TrafficSignalArray = autoware_auto_perception_msgs::msg::TrafficSignalArray;
+  using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight;
+  using MarkerArray = visualization_msgs::msg::MarkerArray;
+  using Pose = geometry_msgs::msg::Pose;
+
+  explicit TrafficLightRecognitionMarkerPublisher(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_ptr_;
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::TrafficSignalArray>::SharedPtr
+    sub_tlr_ptr_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_ptr_;
+
+  void onMap(const HADMapBin::ConstSharedPtr msg_ptr);
+  void onTrafficSignalArray(const TrafficSignalArray::ConstSharedPtr msg_ptr);
+  visualization_msgs::msg::Marker getTrafficLightMarker(
+    const Pose & tl_pose, const uint8_t tl_color, const uint8_t tl_shape);
+
+  std::string getTrafficLightString(const uint8_t tl_color, const uint8_t tl_shape);
+  std_msgs::msg::ColorRGBA getTrafficLightColor(const uint8_t tl_color, const uint8_t tl_shape);
+
+  std::map<int32_t, Pose> tl_position_map_;
+  bool is_map_ready_ = false;
+  int32_t marker_id = 0;
+};
+
+#endif  // TRAFFIC_LIGHT_RECOGNITION_MARKER_PUBLISHER_HPP_

perception/tensorrt_yolox/src/tensorrt_yolox.cpp

@@ -428,7 +428,8 @@ bool TrtYoloX::feedforward(const std::vector<cv::Mat> & images, ObjectArrays & o
   objects.clear();
   for (size_t i = 0; i < batch_size; ++i) {
     const size_t num_detection = static_cast<size_t>(out_num_detections[i]);
-    ObjectArray object_array(num_detection);
+    ObjectArray object_array;
+    object_array.reserve(num_detection);
     for (size_t j = 0; j < num_detection; ++j) {
       Object object{};
       const auto x1 = out_boxes[i * max_detections_ * 4 + j * 4] / scales_[i];

perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp

@@ -199,6 +199,8 @@ void TrtYoloXNode::replaceLabelMap()
     auto & label = label_map_[i];
     if (label == "PERSON") {
       label = "PEDESTRIAN";
+    } else if (label == "MOTORBIKE") {
+      label = "MOTORCYCLE";
     } else if (
       label != "CAR" && label != "PEDESTRIAN" && label != "BUS" && label != "TRUCK" &&
       label != "BICYCLE" && label != "MOTORCYCLE") {