feat(crosswalk_traffic_light_estimator): add new package for crosswal…

…k traffic light signals estimation (tier4#944)

* feat(traffic_light_estimator): add pedestrian traffic signal estimator

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(traffic_light_estimator): enable was_green

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(traffic_light_estimator): support specialize tag

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(traffic_light_estimator): cleanup callback

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(traffic_light_estimator): fix typo

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(traffic_light_estimator): pass by reference

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(traffic_light_estimator): fix copyright

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(traffic_light_estimator): modify build error

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(traffic_light_estimator): fix build error

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(traffic_light_estimator): fix input topic name

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(traffic_light_estimator): add flag to use last detect signal

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* docs(traffic_light_estimator): add README

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* docs(traffic_light_estimator): update flow diagram

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(crosswalk_traffic_light_estimator): package rename

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

perception/crosswalk_traffic_light_estimator/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(crosswalk_traffic_light_estimator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+###########
+## Build ##
+###########
+
+include_directories()
+
+ament_auto_add_library(crosswalk_traffic_light_estimator SHARED
+  src/node.cpp
+)
+
+rclcpp_components_register_node(crosswalk_traffic_light_estimator
+  PLUGIN "traffic_light::CrosswalkTrafficLightEstimatorNode"
+  EXECUTABLE crosswalk_traffic_light_estimator_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+#############
+## Install ##
+#############
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

perception/crosswalk_traffic_light_estimator/README.md

@@ -0,0 +1,83 @@
+# crosswalk_traffic_light_estimator
+
+## Purpose
+
+`crosswalk_traffic_light_estimator` is a module that estimates pedestrian traffic signals from HDMap and detected vehicle traffic signals.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                 | Type                                                     | Description        |
+| ------------------------------------ | -------------------------------------------------------- | ------------------ |
+| `~/input/vector_map`                 | `autoware_auto_mapping_msgs::msg::HADMapBin`             | vector map         |
+| `~/input/route`                      | `autoware_auto_planning_msgs::msg::HADMapRoute`          | route              |
+| `~/input/classified/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | classified signals |
+
+### Output
+
+| Name                       | Type                                                     | Description                                               |
+| -------------------------- | -------------------------------------------------------- | --------------------------------------------------------- |
+| `~/output/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | output that contains estimated pedestrian traffic signals |
+
+## Parameters
+
+| Name                    | Type   | Description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      | Default value |
+| :---------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN but also when detection results change GREEN to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN, this module estimates pedestrian's traffic signal as RED.) | `true`        |
+
+## Inner-workings / Algorithms
+
+```plantuml
+
+start
+:subscribe detected traffic signals & HDMap;
+:extract crosswalk lanelets from HDMap;
+:extract road lanelets that conflicts crosswalk;
+:initialize green_lanelets(lanelet::ConstLanelets);
+if (Latest detection result is **GREEN**?) then (yes)
+  :push back green_lanelets;
+else (no)
+  if (use_last_detect_color is **true**?) then (yes)
+    if (Latest detection result is **UNKNOWN** and last detection result is **GREEN**?) then (yes)
+     :push back green_lanelets;
+    endif
+  endif
+endif
+if (Is there **STRAIGHT-GREEN** road lanelet in green_lanelets?) then (yes)
+  :estimate related pedestrian's traffic signal as **RED**;
+else if (Is there both **LEFT-GREEN** and **RIGHT-GREEN** road lanelet in green_lanelets?) then (yes)
+  :estimate related pedestrian's traffic signal as **RED**;
+else (no)
+  :estimate related pedestrian's traffic signal as **UNKNOWN**;
+endif
+end
+
+```
+
+If traffic between pedestrians and vehicles is controlled by traffic signals, the crosswalk traffic signal maybe **RED** in order to prevent pedestrian from crossing when the following conditions are satisfied.
+
+### Situation1
+
+- crosswalk conflicts **STRAIGHT** lanelet
+- the lanelet refers **GREEN** traffic signal
+
+<div align="center">
+  <img src="images/straight.drawio.svg" width=80%>
+</div>
+<div align="center">
+  <img src="images/intersection1.svg" width=80%>
+</div>
+
+### Situation2
+
+- crosswalk conflicts different turn direction lanelets (STRAIGHT and LEFT, LEFT and RIGHT, RIGHT and STRAIGHT)
+- the lanelets refer **GREEN** traffic signal
+
+<div align="center">
+  <img src="images/intersection2.svg" width=80%>
+</div>
+
+## Assumptions / Known limits
+
+## Future extensions / Unimplemented parts