tools: migrated manual_traffic_light

apollo.sh

@@ -111,7 +111,8 @@ function generate_build_targets() {
     //modules/map/hdmap/...
     //modules/planning/...
     //modules/canbus/...
-    //modules/third_party_perception/..."
+    //modules/third_party_perception/...
+    //modules/tools/manual_traffic_light/..."
   # //modules/drivers/radar/conti_radar/...
   # //modules/drivers/gnss/...
   # //modules/monitor/...

modules/planning/BUILD

@@ -25,19 +25,19 @@ cc_library(
 
 cc_binary(
     name = "libplanning_component.so",
-    deps = [":planning_component_lib"],
     linkopts = ["-shared"],
+    deps = [":planning_component_lib"],
 )
 
 cc_library(
     name = "planning_lib",
     srcs = [
-        "std_planning.cc",
         "planning_base.cc",
+        "std_planning.cc",
     ],
     hdrs = [
-        "std_planning.h",
         "planning_base.h",
+        "std_planning.h",
     ],
     deps = [
         "//modules/common",

modules/tools/manual_traffic_light/BUILD

@@ -2,12 +2,14 @@ load("//tools:cpplint.bzl", "cpplint")
 
 package(default_visibility = ["//visibility:public"])
 
-cc_binary(
+cc_library(
     name = "manual_traffic_light",
     srcs = ["manual_traffic_light.cc"],
+    copts = ["-DMODULE_NAME=\\\"manual_traffic_light\\\""],
     data = ["//modules/map:map_data"],
     deps = [
         "//external:gflags",
+        "//framework:cybertron",
         "//modules/common",
         "//modules/common/configs:config_gflags",
         "//modules/common/util",
@@ -17,4 +19,10 @@ cc_binary(
     ],
 )
 
+cc_binary(
+    name = "libmanual_traffic_light_component.so",
+    linkopts = ["-shared"],
+    deps = [":manual_traffic_light"],
+)
+
 cpplint()

modules/tools/manual_traffic_light/manual_traffic_light.cc

@@ -20,18 +20,18 @@
 #include <fstream>
 #include <iostream>
 
+#include "cybertron/component/timer_component.h"
+#include "cybertron/cybertron.h"
 #include "gflags/gflags.h"
-#include "ros/include/ros/ros.h"
 
 #include "modules/localization/proto/localization.pb.h"
 #include "modules/perception/proto/traffic_light_detection.pb.h"
 
-#include "modules/common/adapters/adapter_gflags.h"
-#include "modules/common/adapters/adapter_manager.h"
 #include "modules/common/configs/config_gflags.h"
 #include "modules/common/log.h"
 #include "modules/common/util/color.h"
 #include "modules/common/util/file.h"
+#include "modules/common/util/message_util.h"
 #include "modules/common/util/string_util.h"
 #include "modules/map/hdmap/adapter/opendrive_adapter.h"
 #include "modules/map/hdmap/hdmap_common.h"
@@ -39,9 +39,6 @@
 #include "modules/map/hdmap/hdmap_util.h"
 
 using apollo::common::PointENU;
-using apollo::common::adapter::AdapterConfig;
-using apollo::common::adapter::AdapterManager;
-using apollo::common::adapter::AdapterManagerConfig;
 using apollo::common::color::ANSI_GREEN;
 using apollo::common::color::ANSI_RED;
 using apollo::common::color::ANSI_RESET;
@@ -58,140 +55,23 @@ DEFINE_bool(all_lights, false, "set all lights on the map");
 DEFINE_double(traffic_light_distance, 1000.0,
               "only retrieves traffic lights within this distance");
 
-bool g_is_green = false;
-bool g_updated = true;
-
-bool GetAllTrafficLights(std::vector<SignalInfoConstPtr> *traffic_lights) {
-  static auto map_filename = apollo::hdmap::BaseMapFile();
-  static apollo::hdmap::Map map_proto;
-  static std::vector<SignalInfoConstPtr> map_traffic_lights;
-  if (map_proto.lane_size() == 0 && map_traffic_lights.empty()) {
-    AERROR << "signal size: " << map_proto.signal_size();
-    if (apollo::common::util::EndWith(map_filename, ".xml")) {
-      if (!apollo::hdmap::adapter::OpendriveAdapter::LoadData(map_filename,
-                                                              &map_proto)) {
-        return false;
-      }
-    } else if (!apollo::common::util::GetProtoFromFile(map_filename,
-                                                       &map_proto)) {
-      return false;
-    }
-    for (const auto &signal : map_proto.signal()) {
-      const auto *hdmap = HDMapUtil::BaseMapPtr();
-      if (!hdmap) {
-        AERROR << "Invalid HD Map.";
-        return false;
-      }
-      map_traffic_lights.push_back(hdmap->GetSignalById(signal.id()));
-    }
-  }
-  *traffic_lights = map_traffic_lights;
-  return true;
-}
-
-bool GetTrafficLightsWithinDistance(
-    std::vector<SignalInfoConstPtr> *traffic_lights) {
-  CHECK_NOTNULL(traffic_lights);
-  AdapterManager::Observe();
-  if (AdapterManager::GetLocalization()->Empty()) {
-    AERROR << "No localization received";
-    return false;
-  }
-  const auto *hdmap = HDMapUtil::BaseMapPtr();
-  if (!hdmap) {
-    AERROR << "Invalid HD Map.";
-    return false;
-  }
-  auto position =
-      AdapterManager::GetLocalization()->GetLatestObserved().pose().position();
-  int ret = hdmap->GetForwardNearestSignalsOnLane(
-      position, FLAGS_traffic_light_distance, traffic_lights);
-  if (ret != 0) {
-    AERROR << "failed to get signals from position: "
-           << position.ShortDebugString()
-           << " with distance: " << FLAGS_traffic_light_distance << " on map";
-    return false;
-  }
-  return true;
-}
-
-bool CreateTrafficLightDetection(const std::vector<SignalInfoConstPtr> &signals,
-                                 TrafficLight::Color color,
-                                 TrafficLightDetection *detection) {
-  CHECK_NOTNULL(detection);
-  for (const auto &iter : signals) {
-    auto *light = detection->add_traffic_light();
-    light->set_color(color);
-    light->set_confidence(1.0);
-    light->set_tracking_time(1.0);
-    light->set_id(iter->id().id());
-  }
-  AdapterManager::FillTrafficLightDetectionHeader("manual_traffic_light",
-                                                  detection);
-  return true;
-}
-
-TrafficLight::Color GetKeyBoardColorInput() {
-  int8_t revent = 0;  // short
-  struct pollfd fd = {STDIN_FILENO, POLLIN, revent};
-  switch (poll(&fd, 1, 100)) {
-    case -1:
-      AERROR << "Failed to read keybapoard";
-      break;
-    case 0:
-      break;
-    default:
-      char ch = 'x';
-      std::cin >> ch;
-      if (ch == 'c') {
-        g_is_green = !g_is_green;
-        g_updated = true;
-      }
-      break;
-  }
-  if (g_is_green) {
-    return TrafficLight::GREEN;
-  } else {
-    return TrafficLight::RED;
-  }
-}
-
-bool IsDifferent(const std::unordered_set<std::string> &str_set,
-                 const std::vector<std::string> &str_vec) {
-  if (str_vec.size() != str_set.size()) {
+class ManualTrafficLight final : public ::apollo::cybertron::TimerComponent {
+ public:
+  bool Init() {
+    localization_reader_ = node_->CreateReader<LocalizationEstimate>(
+        "/apollo/localization/pose",
+        [this](const std::shared_ptr<LocalizationEstimate> &localization) {
+          ADEBUG << "Received chassis data: run chassis callback.";
+          OnLocalization(localization);
+        });
+
+    traffic_light_detection_writer_ =
+        node_->CreateWriter<TrafficLightDetection>(
+            "/apollo/perception/traffic_light");
     return true;
   }
-  for (const auto &ss : str_vec) {
-    if (str_set.count(ss) == 0) {
-      return true;
-    }
-  }
-  return false;
-}
-
-int main(int argc, char *argv[]) {
-  google::InitGoogleLogging(argv[0]);
-  google::ParseCommandLineFlags(&argc, &argv, true);
-  ros::init(argc, argv, "manual_traffic_light");
-
-  AdapterManagerConfig manager_config;
-  auto *config = manager_config.add_config();
-  config->set_type(AdapterConfig::TRAFFIC_LIGHT_DETECTION);
-  config->set_mode(AdapterConfig::PUBLISH_ONLY);
-  config->set_message_history_limit(1);
-
-  config = manager_config.add_config();
-  config->set_type(AdapterConfig::LOCALIZATION);
-  config->set_mode(AdapterConfig::RECEIVE_ONLY);
-  config->set_message_history_limit(1);
-  manager_config.set_is_ros(true);
-  AdapterManager::Init(manager_config);
-  std::unordered_set<std::string> prev_traffic_lights;
-
-  ros::Rate loop_rate(10);
-  while (ros::ok()) {
-    ros::spinOnce();
-    loop_rate.sleep();
+
+  bool Proc() {
     std::vector<SignalInfoConstPtr> signals;
     bool result = false;
     if (FLAGS_all_lights) {
@@ -201,7 +81,6 @@ int main(int argc, char *argv[]) {
     }
     if (!result) {
       ADEBUG << "Failed to get traffic signals from current location on map";
-      continue;
     } else {
       ADEBUG << "Received " << signals.size() << " traffic lights";
     }
@@ -209,19 +88,19 @@ int main(int argc, char *argv[]) {
     for (const auto &ptr : signals) {
       signal_ids.emplace_back(ptr->id().id());
     }
-    if (IsDifferent(prev_traffic_lights, signal_ids)) {
-      prev_traffic_lights =
+    if (IsDifferent(prev_traffic_lights_, signal_ids)) {
+      prev_traffic_lights_ =
           std::unordered_set<std::string>(signal_ids.begin(), signal_ids.end());
-      g_updated = true;
+      updated_ = true;
     }
     TrafficLightDetection traffic_light_detection;
     TrafficLight::Color color = GetKeyBoardColorInput();
     ADEBUG << "Color: " << TrafficLight::Color_Name(color);
-    if (g_updated) {
-      g_updated = false;
-      const char *print_color = g_is_green ? ANSI_GREEN : ANSI_RED;
+    if (updated_) {
+      updated_ = false;
+      const char *print_color = is_green_ ? ANSI_GREEN : ANSI_RED;
       std::cout << print_color
-                << "Current Light: " << (g_is_green ? "GREEN" : "RED");
+                << "Current Light: " << (is_green_ ? "GREEN" : "RED");
       if (signal_ids.empty()) {
         if (FLAGS_all_lights) {
           std::cout << " No lights in the map";
@@ -244,7 +123,132 @@ int main(int argc, char *argv[]) {
                 << std::endl;
     }
     CreateTrafficLightDetection(signals, color, &traffic_light_detection);
-    AdapterManager::PublishTrafficLightDetection(traffic_light_detection);
+    traffic_light_detection_writer_->Write(traffic_light_detection);
+    return true;
+  }
+
+ private:
+  bool GetAllTrafficLights(std::vector<SignalInfoConstPtr> *traffic_lights) {
+    static auto map_filename = apollo::hdmap::BaseMapFile();
+    static apollo::hdmap::Map map_proto;
+    static std::vector<SignalInfoConstPtr> map_traffic_lights;
+    if (map_proto.lane_size() == 0 && map_traffic_lights.empty()) {
+      AERROR << "signal size: " << map_proto.signal_size();
+      if (apollo::common::util::EndWith(map_filename, ".xml")) {
+        if (!apollo::hdmap::adapter::OpendriveAdapter::LoadData(map_filename,
+                                                                &map_proto)) {
+          return false;
+        }
+      } else if (!apollo::common::util::GetProtoFromFile(map_filename,
+                                                         &map_proto)) {
+        return false;
+      }
+      for (const auto &signal : map_proto.signal()) {
+        const auto *hdmap = HDMapUtil::BaseMapPtr();
+        if (!hdmap) {
+          AERROR << "Invalid HD Map.";
+          return false;
+        }
+        map_traffic_lights.push_back(hdmap->GetSignalById(signal.id()));
+      }
+    }
+    *traffic_lights = map_traffic_lights;
+    return true;
+  }
+
+  bool GetTrafficLightsWithinDistance(
+      std::vector<SignalInfoConstPtr> *traffic_lights) {
+    CHECK_NOTNULL(traffic_lights);
+    if (!has_localization_) {
+      AERROR << "No localization received";
+      return false;
+    }
+    const auto *hdmap = HDMapUtil::BaseMapPtr();
+    if (!hdmap) {
+      AERROR << "Invalid HD Map.";
+      return false;
+    }
+    auto position = localization_.pose().position();
+    int ret = hdmap->GetForwardNearestSignalsOnLane(
+        position, FLAGS_traffic_light_distance, traffic_lights);
+    if (ret != 0) {
+      AERROR << "failed to get signals from position: "
+             << position.ShortDebugString()
+             << " with distance: " << FLAGS_traffic_light_distance << " on map";
+      return false;
+    }
+    return true;
+  }
+
+  bool CreateTrafficLightDetection(
+      const std::vector<SignalInfoConstPtr> &signals, TrafficLight::Color color,
+      TrafficLightDetection *detection) {
+    CHECK_NOTNULL(detection);
+    for (const auto &iter : signals) {
+      auto *light = detection->add_traffic_light();
+      light->set_color(color);
+      light->set_confidence(1.0);
+      light->set_tracking_time(1.0);
+      light->set_id(iter->id().id());
+    }
+    ::apollo::common::util::FillHeader("manual_traffic_light", detection);
+    return true;
   }
-  return 0;
-}
+
+  TrafficLight::Color GetKeyBoardColorInput() {
+    int8_t revent = 0;  // short
+    struct pollfd fd = {STDIN_FILENO, POLLIN, revent};
+    switch (poll(&fd, 1, 100)) {
+      case -1:
+        AERROR << "Failed to read keybapoard";
+        break;
+      case 0:
+        break;
+      default:
+        char ch = 'x';
+        std::cin >> ch;
+        if (ch == 'c') {
+          is_green_ = !is_green_;
+          updated_ = true;
+        }
+        break;
+    }
+    if (is_green_) {
+      return TrafficLight::GREEN;
+    } else {
+      return TrafficLight::RED;
+    }
+  }
+
+  bool IsDifferent(const std::unordered_set<std::string> &str_set,
+                   const std::vector<std::string> &str_vec) {
+    if (str_vec.size() != str_set.size()) {
+      return true;
+    }
+    for (const auto &ss : str_vec) {
+      if (str_set.count(ss) == 0) {
+        return true;
+      }
+    }
+    return false;
+  }
+
+  void OnLocalization(
+      const std::shared_ptr<LocalizationEstimate> &localization) {
+    localization_ = *localization;
+    has_localization_ = true;
+  }
+
+ private:
+  bool is_green_ = false;
+  bool updated_ = true;
+  bool has_localization_ = false;
+  LocalizationEstimate localization_;
+  std::unordered_set<std::string> prev_traffic_lights_;
+  std::shared_ptr<::apollo::cybertron::Writer<TrafficLightDetection>>
+      traffic_light_detection_writer_ = nullptr;
+  std::shared_ptr<::apollo::cybertron::Reader<LocalizationEstimate>>
+      localization_reader_ = nullptr;
+};
+
+CYBERTRON_REGISTER_COMPONENT(ManualTrafficLight);

modules/tools/manual_traffic_light/manual_traffic_light.dag

+# Define all coms in DAG streaming.
+module_config {
+  module_library : "/apollo/bazel-bin/modules/tools/manual_traffic_light/libmanual_traffic_light_component.so"
+  timer_components {
+    class_name : "ManualTrafficLight"
+    config {
+      name: "manual_traffic_light_componenet"
+      interval: 200 # milliseconds
+    }
+  }
+}

modules/tools/manual_traffic_light/manual_traffic_light.launch

+<cybertron>
+    <module>
+        <name>manual_traffic_light</name>
+        <dag_conf>/apollo/modules/tools/manual_traffic_light/manual_traffic_light.dag</dag_conf>
+        <process_name>manual_traffic_light</process_name>
+    </module>
+</cybertron>