From a04d30da2b51399e98f734c12e42f2441cf182e4 Mon Sep 17 00:00:00 2001 From: QIU1995NONAME <292529702@qq.com> Date: Fri, 4 Sep 2020 13:32:29 +0800 Subject: [PATCH] V2X: add proto adapter --- modules/v2x/BUILD | 1 - modules/v2x/common/v2x_proxy_gflags.cc | 10 +- modules/v2x/proto/v2x_obu_traffic_light.proto | 4 +- modules/v2x/proto/v2x_traffic_light.proto | 2 +- modules/v2x/v2x_proxy/proto_adapter/BUILD | 35 +++ .../v2x_proxy/proto_adapter/proto_adapter.cc | 235 ++++++++++++++++++ .../v2x_proxy/proto_adapter/proto_adapter.h | 74 ++++++ .../proto_adapter/proto_adapter_test.cc | 40 +++ 8 files changed, 392 insertions(+), 9 deletions(-) create mode 100644 modules/v2x/v2x_proxy/proto_adapter/BUILD create mode 100644 modules/v2x/v2x_proxy/proto_adapter/proto_adapter.cc create mode 100644 modules/v2x/v2x_proxy/proto_adapter/proto_adapter.h create mode 100644 modules/v2x/v2x_proxy/proto_adapter/proto_adapter_test.cc diff --git a/modules/v2x/BUILD b/modules/v2x/BUILD index 00fa2b9226..2dd3595b4e 100644 --- a/modules/v2x/BUILD +++ b/modules/v2x/BUILD @@ -1,4 +1,3 @@ -load("@rules_cc//cc:defs.bzl", "cc_binary") load("//tools:cpplint.bzl", "cpplint") package(default_visibility = ["//visibility:public"]) diff --git a/modules/v2x/common/v2x_proxy_gflags.cc b/modules/v2x/common/v2x_proxy_gflags.cc index fe7257054e..55515c2a30 100644 --- a/modules/v2x/common/v2x_proxy_gflags.cc +++ b/modules/v2x/common/v2x_proxy_gflags.cc @@ -47,11 +47,11 @@ DEFINE_int64(msg_timeout, 250, "timeout value which getting the msg from OBU"); DEFINE_int64(sim_sending_num, 10, "the max sending times"); DEFINE_bool(use_nearest_flag, true, "use the hdmap interface get_forward_nearest_signals_on_lane flag"); -DEFINE_int64(spat_period, 150, "SPAT message periond"); -DEFINE_double(check_time, 0.5, "SPAT message periond"); -DEFINE_int64(rsu_whitelist_period, 3 * 1000, "get whitelist periond"); // 3s -DEFINE_string(rsu_whitelist_name, - "/apollo/modules/v2x/conf/rsu_whitelist.txt", +DEFINE_int64(spat_period, 150, "SPAT message period in ms"); +DEFINE_double(check_time, 0.5, "SPAT message period in s"); +DEFINE_int64(rsu_whitelist_period, 3 * 1000, + "get whitelist period in ms"); // 3s +DEFINE_string(rsu_whitelist_name, "/apollo/modules/v2x/conf/rsu_whitelist.txt", "file name for RSU whitelist"); } // namespace v2x diff --git a/modules/v2x/proto/v2x_obu_traffic_light.proto b/modules/v2x/proto/v2x_obu_traffic_light.proto index 7ee916362f..7195cfd5aa 100644 --- a/modules/v2x/proto/v2x_obu_traffic_light.proto +++ b/modules/v2x/proto/v2x_obu_traffic_light.proto @@ -14,7 +14,7 @@ message SingleTrafficLight { }; optional apollo.v2x.SingleTrafficLight.Color color = 1; - optional int32 trafficlight_type = 2; + optional int32 traffic_light_type = 2; // Traffic light string-ID in the map data. optional string id = 3; optional int32 color_remaining_time_s = 4; @@ -36,7 +36,7 @@ message LaneTrafficLight { } message RoadTrafficLight { - repeated LaneTrafficLight lane_trafficlight = 1; + repeated LaneTrafficLight lane_traffic_light = 1; optional int32 road_direction = 2; } message OBUTrafficLightData { diff --git a/modules/v2x/proto/v2x_traffic_light.proto b/modules/v2x/proto/v2x_traffic_light.proto index 2ffaf0c527..b125d82e3d 100644 --- a/modules/v2x/proto/v2x_traffic_light.proto +++ b/modules/v2x/proto/v2x_traffic_light.proto @@ -20,7 +20,7 @@ message SingleTrafficLight { U_TURN = 3; }; optional Color color = 1; - repeated Type trafficlight_type = 2; + repeated Type traffic_light_type = 2; // Traffic light string-ID in the map data. optional string id = 3; optional int32 color_remaining_time_s = 4; diff --git a/modules/v2x/v2x_proxy/proto_adapter/BUILD b/modules/v2x/v2x_proxy/proto_adapter/BUILD new file mode 100644 index 0000000000..6a51a412c9 --- /dev/null +++ b/modules/v2x/v2x_proxy/proto_adapter/BUILD @@ -0,0 +1,35 @@ +load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test") +load("//tools:cpplint.bzl", "cpplint") + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "proto_adapter", + srcs = ["proto_adapter.cc"], + hdrs = ["proto_adapter.h"], + deps = [ + "//cyber", + "//modules/map/hdmap", + "//modules/v2x/common:v2x_proxy_gflags", + "//modules/v2x/proto:v2x_carstatus_cc_proto", + "//modules/v2x/proto:v2x_junction_cc_proto", + "//modules/v2x/proto:v2x_obu_rsi_cc_proto", + "//modules/v2x/proto:v2x_obu_traffic_light_cc_proto", + "//modules/v2x/proto:v2x_rsi_cc_proto", + "//modules/v2x/proto:v2x_traffic_light_cc_proto", + "@eigen", + ], +) + +cc_test( + name = "proto_adapter_test", + size = "small", + timeout = "short", + srcs = ["proto_adapter_test.cc"], + deps = [ + ":proto_adapter", + "@com_google_googletest//:gtest_main", + ], +) + +cpplint() diff --git a/modules/v2x/v2x_proxy/proto_adapter/proto_adapter.cc b/modules/v2x/v2x_proxy/proto_adapter/proto_adapter.cc new file mode 100644 index 0000000000..24965995fd --- /dev/null +++ b/modules/v2x/v2x_proxy/proto_adapter/proto_adapter.cc @@ -0,0 +1,235 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * 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 "modules/v2x/v2x_proxy/proto_adapter/proto_adapter.h" + +#include +#include + +#include "cyber/cyber.h" + +namespace apollo { +namespace v2x { + +OSLightype ProtoAdapter::LightTypeObu2Sys(int32_t type) { + switch (type) { + case 2: + return OSLightype::SingleTrafficLight_Type_LEFT; + case 3: + return OSLightype::SingleTrafficLight_Type_RIGHT; + case 4: + return OSLightype::SingleTrafficLight_Type_U_TURN; + case 1: + default: + return OSLightype::SingleTrafficLight_Type_STRAIGHT; + } +} + +bool ProtoAdapter::LightObu2Sys(const OBULight &obu_light, + std::shared_ptr *os_light) { +#if 0 + if (nullptr == os_light) { + return false; + } + auto res = std::make_shared(); + if (nullptr == res) { + return false; + } + if (!obu_light.has_header()) { + return false; + } + if (obu_light.header().has_timestamp_sec()) { + res->mutable_header()->set_timestamp_sec( + obu_light.header().timestamp_sec()); + } + if (obu_light.header().has_module_name()) { + res->mutable_header()->set_module_name(obu_light.header().module_name()); + } + if (obu_light.road_trafficlight_size() < 1) { + return false; + } + // FOR-EACH ROAD + bool flag_has_data = false; + for (int idx_road = 0; idx_road < obu_light.road_trafficlight_size(); + idx_road++) { + const auto &obu_road_light1 = obu_light.road_trafficlight(idx_road); + // Set the road index for lane + apollo::v2x::RoadTrafficLight_Attribute tl_attr = + apollo::v2x::RoadTrafficLight_Attribute_EAST; + switch (obu_road_light1.road_direction()) { + case 1: + tl_attr = apollo::v2x::RoadTrafficLight_Attribute_EAST; + break; + case 2: + tl_attr = apollo::v2x::RoadTrafficLight_Attribute_WEST; + break; + case 3: + tl_attr = apollo::v2x::RoadTrafficLight_Attribute_SOUTH; + break; + case 4: + tl_attr = apollo::v2x::RoadTrafficLight_Attribute_NORTH; + break; + default: + AINFO << "Road direction=" << obu_road_light1.road_direction() + << " is invalid."; + } + // FOR-EACH LANE + for (int idx_lane = 0; idx_lane < obu_road_light1.lane_traffic_light_size(); + idx_lane++) { + const auto &obu_lane_light1 = + obu_road_light1.lane_traffic_light(idx_lane); + if (!obu_lane_light1.has_gps_x_m() || !obu_lane_light1.has_gps_y_m()) { + AWARN << "Invalid lane_traffic_light: [" << idx_road << "][" << idx_lane + << "]: no gps info here."; + continue; + } + flag_has_data = true; + auto *res_light1 = res->add_road_traffic_light(); + res_light1->set_gps_x_m(obu_lane_light1.gps_x_m()); + res_light1->set_gps_y_m(obu_lane_light1.gps_y_m()); + res_light1->set_road_attribute(tl_attr); + // single traffic light + for (int j = 0; j < obu_lane_light1.single_traffic_light_size(); j++) { + auto *res_single1 = res_light1->add_single_traffic_light(); + const auto &obu_single1 = obu_lane_light1.single_traffic_light(j); + if (obu_single1.has_id()) { + res_single1->set_id(obu_single1.id()); + } + if (obu_single1.has_color_remaining_time_s()) { + res_single1->set_color_remaining_time_s( + obu_single1.color_remaining_time_s()); + } + if (obu_single1.has_next_remaining_time()) { + res_single1->set_next_remaining_time_s( + obu_single1.next_remaining_time()); + } + if (obu_single1.has_right_turn_light()) { + res_single1->set_right_turn_light(obu_single1.right_turn_light()); + } + if (obu_single1.has_color()) { + res_single1->set_color(obu_single1.color()); + } + if (obu_single1.has_next_color()) { + res_single1->set_next_color(obu_single1.next_color()); + } + if (obu_single1.has_traffic_light_type()) { + res_single1->add_traffic_light_type( + LightTypeObu2Sys(obu_single1.traffic_light_type())); + } + } + } + } + *os_light = res; + return flag_has_data; +#endif + return false; +} + +bool ProtoAdapter::RsiObu2Sys(const OBURsi *obu_rsi, + std::shared_ptr *os_rsi) { + if (nullptr == obu_rsi) { + return false; + } + if (nullptr == os_rsi) { + return false; + } + auto res = std::make_shared(); + if (nullptr == res) { + return false; + } + if (!obu_rsi->has_alter_type()) { + return false; + } + res->set_radius(obu_rsi->radius()); + res->set_rsi_type(obu_rsi->alter_type()); + if (obu_rsi->has_header()) { + auto header = res->mutable_header(); + header->set_sequence_num(obu_rsi->header().sequence_num()); + header->set_timestamp_sec(obu_rsi->header().timestamp_sec()); + header->set_module_name("v2x"); + } + switch (obu_rsi->alter_type()) { + case RsiAlterType::SPEED_LIMIT: + case RsiAlterType::SPEED_LIMIT_BRIDGE: + case RsiAlterType::SPEED_LIMIT_TUNNEL: + for (int index = 0; index < obu_rsi->points_size(); index++) { + auto point = res->add_points(); + point->set_x(obu_rsi->points(index).x()); + point->set_y(obu_rsi->points(index).y()); + res->set_speed(std::atof(obu_rsi->description().c_str())); + } + break; + case RsiAlterType::CONSTRUCTION_AHEAD: // Construction Ahead + case RsiAlterType::BUS_LANE: // Bus Lane + case RsiAlterType::TIDAL_LANE: // tidal Lane + case RsiAlterType::TRAFFIC_JAM: // traffic jam + case RsiAlterType::TRAFFIC_ACCIDENT: // traffic accident + for (int index = 0; index < obu_rsi->points_size(); index++) { + auto point = res->add_points(); + point->set_x(obu_rsi->points(index).x()); + point->set_y(obu_rsi->points(index).y()); + } + break; + case RsiAlterType::NO_HONKING: // No Honking + case RsiAlterType::SLOW_DOWN_SECTION: // slow down section + case RsiAlterType::ACCIDENT_PRONE: // Accident Prone + case RsiAlterType::OVERSPEED_VEHICLE: // overspeed vehicle + case RsiAlterType::EMERGENCY_BRAKING: // emergency braking + case RsiAlterType::ANTIDROMIC_VEHICLE: // antidromic vehicle + case RsiAlterType::ZOMBIES_VEHICLE: { // zombies vehicle + auto point = res->add_points(); + point->set_x(obu_rsi->points(0).x()); + point->set_y(obu_rsi->points(0).y()); + break; + } + case RsiAlterType::CONTROLLOSS_VEHICLE: // controlloss vehicle + case RsiAlterType::SPECIAL_VEHICLE: { // special vehicle + auto point = res->add_points(); + point->set_x(obu_rsi->points(0).x()); + point->set_y(obu_rsi->points(0).y()); + res->set_speed(std::atof(obu_rsi->description().c_str())); + break; + } + default: + AINFO << "RSI type:" << obu_rsi->alter_type(); + break; + } + *os_rsi = res; + return true; +} + +bool ProtoAdapter::JunctionHd2obu(const HDJunction &hd_junction, + std::shared_ptr *obu_junction) { + if (nullptr == obu_junction) { + return false; + } + auto res = std::make_shared(); + if (nullptr == res) { + return false; + } + res->mutable_id()->set_id(hd_junction->id().id()); + for (const auto &point : hd_junction->polygon().points()) { + auto res_point = res->mutable_polygon()->add_point(); + res_point->set_x(point.x()); + res_point->set_y(point.y()); + res_point->set_z(0); + } + *obu_junction = res; + return true; +} + +} // namespace v2x +} // namespace apollo diff --git a/modules/v2x/v2x_proxy/proto_adapter/proto_adapter.h b/modules/v2x/v2x_proxy/proto_adapter/proto_adapter.h new file mode 100644 index 0000000000..dd2ed43209 --- /dev/null +++ b/modules/v2x/v2x_proxy/proto_adapter/proto_adapter.h @@ -0,0 +1,74 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * 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. + *****************************************************************************/ + +#pragma once + +#include + +#include "modules/v2x/proto/v2x_carstatus.pb.h" +#include "modules/v2x/proto/v2x_junction.pb.h" +#include "modules/v2x/proto/v2x_obu_rsi.pb.h" +#include "modules/v2x/proto/v2x_obu_traffic_light.pb.h" +#include "modules/v2x/proto/v2x_rsi.pb.h" +#include "modules/v2x/proto/v2x_traffic_light.pb.h" + +#include "modules/map/hdmap/hdmap.h" +#include "modules/map/hdmap/hdmap_common.h" + +namespace apollo { +namespace v2x { +enum RsiAlterType { + SPEED_LIMIT = 85, + SPEED_LIMIT_BRIDGE = 8, + SPEED_LIMIT_TUNNEL = 21, + CONSTRUCTION_AHEAD = 38, + BUS_LANE = 123, + TIDAL_LANE = 41, + TRAFFIC_JAM = 47, + TRAFFIC_ACCIDENT = 244, + NO_HONKING = 80, + SLOW_DOWN_SECTION = 35, + ACCIDENT_PRONE = 34, + OVERSPEED_VEHICLE = 801, + EMERGENCY_BRAKING = 802, + ANTIDROMIC_VEHICLE = 803, + ZOMBIES_VEHICLE = 804, + CONTROLLOSS_VEHICLE = 1000, + SPECIAL_VEHICLE = 2000, +}; + +using OSLightColor = ::apollo::v2x::SingleTrafficLight_Color; +using OSLightype = ::apollo::v2x::SingleTrafficLight_Type; +using OBULightype = ::apollo::v2x::obu::SingleTrafficLight_Type; +using OSLight = ::apollo::v2x::IntersectionTrafficLightData; +using OBULight = ::apollo::v2x::obu::OBUTrafficLightData; +using OSLocation = ::apollo::localization::LocalizationEstimate; +using OSRsi = ::apollo::v2x::RsiMsg; +using OBURsi = ::apollo::v2x::obu::OBURsi; +using HDJunction = ::apollo::hdmap::JunctionInfoConstPtr; +using OBUJunction = ::apollo::v2x::Junction; +class ProtoAdapter final { + public: + static OSLightype LightTypeObu2Sys(int32_t type); + static bool LightObu2Sys(const OBULight &obu_light, + std::shared_ptr *os_light); + static bool RsiObu2Sys(const OBURsi *obu_rsi, std::shared_ptr *os_rsi); + static bool JunctionHd2obu(const HDJunction &hd_junction, + std::shared_ptr *obu_junction); +}; + +} // namespace v2x +} // namespace apollo diff --git a/modules/v2x/v2x_proxy/proto_adapter/proto_adapter_test.cc b/modules/v2x/v2x_proxy/proto_adapter/proto_adapter_test.cc new file mode 100644 index 0000000000..7875fb71ae --- /dev/null +++ b/modules/v2x/v2x_proxy/proto_adapter/proto_adapter_test.cc @@ -0,0 +1,40 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * 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. + *****************************************************************************/ + +/** + * @file proto_adapter_test.cc + */ + +#include "modules/v2x/v2x_proxy/proto_adapter/proto_adapter.h" + +#include "gtest/gtest.h" + +namespace apollo { +namespace v2x { + +TEST(v2x_proxy_app, v2x_proxy_adapter_convert_enum) { + EXPECT_EQ(OSLightype::SingleTrafficLight_Type_LEFT, + ProtoAdapter::LightTypeObu2Sys(2)); + EXPECT_EQ(OSLightype::SingleTrafficLight_Type_RIGHT, + ProtoAdapter::LightTypeObu2Sys(3)); + EXPECT_EQ(OSLightype::SingleTrafficLight_Type_U_TURN, + ProtoAdapter::LightTypeObu2Sys(4)); + EXPECT_EQ(OSLightype::SingleTrafficLight_Type_STRAIGHT, + ProtoAdapter::LightTypeObu2Sys(1)); +} + +} // namespace v2x +} // namespace apollo -- GitLab