From 662e6496df69a0a65fb80e9fe4880b89a170c64e Mon Sep 17 00:00:00 2001 From: Dong Li Date: Fri, 14 Sep 2018 12:57:16 -0700 Subject: [PATCH] tools: migrated manual_traffic_light --- apollo.sh | 3 +- modules/planning/BUILD | 6 +- modules/tools/manual_traffic_light/BUILD | 10 +- .../manual_traffic_light.cc | 302 +++++++++--------- .../manual_traffic_light.dag | 11 + .../manual_traffic_light.launch | 7 + 6 files changed, 185 insertions(+), 154 deletions(-) create mode 100644 modules/tools/manual_traffic_light/manual_traffic_light.dag create mode 100644 modules/tools/manual_traffic_light/manual_traffic_light.launch diff --git a/apollo.sh b/apollo.sh index b9fc920bc4..d10754d400 100755 --- a/apollo.sh +++ b/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/... diff --git a/modules/planning/BUILD b/modules/planning/BUILD index 7fc1cce827..058d788786 100644 --- a/modules/planning/BUILD +++ b/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", diff --git a/modules/tools/manual_traffic_light/BUILD b/modules/tools/manual_traffic_light/BUILD index b2acdc863b..0913309cc5 100644 --- a/modules/tools/manual_traffic_light/BUILD +++ b/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() diff --git a/modules/tools/manual_traffic_light/manual_traffic_light.cc b/modules/tools/manual_traffic_light/manual_traffic_light.cc index 92157b5348..ab453e0638 100644 --- a/modules/tools/manual_traffic_light/manual_traffic_light.cc +++ b/modules/tools/manual_traffic_light/manual_traffic_light.cc @@ -20,18 +20,18 @@ #include #include +#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 *traffic_lights) { - static auto map_filename = apollo::hdmap::BaseMapFile(); - static apollo::hdmap::Map map_proto; - static std::vector 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 *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 &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 &str_set, - const std::vector &str_vec) { - if (str_vec.size() != str_set.size()) { +class ManualTrafficLight final : public ::apollo::cybertron::TimerComponent { + public: + bool Init() { + localization_reader_ = node_->CreateReader( + "/apollo/localization/pose", + [this](const std::shared_ptr &localization) { + ADEBUG << "Received chassis data: run chassis callback."; + OnLocalization(localization); + }); + + traffic_light_detection_writer_ = + node_->CreateWriter( + "/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 prev_traffic_lights; - - ros::Rate loop_rate(10); - while (ros::ok()) { - ros::spinOnce(); - loop_rate.sleep(); + + bool Proc() { std::vector 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(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 *traffic_lights) { + static auto map_filename = apollo::hdmap::BaseMapFile(); + static apollo::hdmap::Map map_proto; + static std::vector 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 *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 &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 &str_set, + const std::vector &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 &localization) { + localization_ = *localization; + has_localization_ = true; + } + + private: + bool is_green_ = false; + bool updated_ = true; + bool has_localization_ = false; + LocalizationEstimate localization_; + std::unordered_set prev_traffic_lights_; + std::shared_ptr<::apollo::cybertron::Writer> + traffic_light_detection_writer_ = nullptr; + std::shared_ptr<::apollo::cybertron::Reader> + localization_reader_ = nullptr; +}; + +CYBERTRON_REGISTER_COMPONENT(ManualTrafficLight); diff --git a/modules/tools/manual_traffic_light/manual_traffic_light.dag b/modules/tools/manual_traffic_light/manual_traffic_light.dag new file mode 100644 index 0000000000..cd7e48dab3 --- /dev/null +++ b/modules/tools/manual_traffic_light/manual_traffic_light.dag @@ -0,0 +1,11 @@ +# 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 + } + } +} diff --git a/modules/tools/manual_traffic_light/manual_traffic_light.launch b/modules/tools/manual_traffic_light/manual_traffic_light.launch new file mode 100644 index 0000000000..b5ecedf593 --- /dev/null +++ b/modules/tools/manual_traffic_light/manual_traffic_light.launch @@ -0,0 +1,7 @@ + + + manual_traffic_light + /apollo/modules/tools/manual_traffic_light/manual_traffic_light.dag + manual_traffic_light + + -- GitLab