diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index b23bcc2a398..6845404af94 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -162,7 +162,8 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode "obstacle", "ego", traffic_simulator::helper::constructPose(10, 5, 0, 0, 0, -1.57), traffic_simulator::helper::constructActionStatus()); - api_.getConventionalTrafficLight(34802).emplace(traffic_simulator::TrafficLight::Color::green); + api_.getConventionalTrafficLights()->setTrafficLightsColor( + 34802, traffic_simulator::TrafficLight::Color::green); } private: diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 77d360107d4..628b8b61c12 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -645,40 +645,57 @@ class SimulatorCore } template - static auto getConventionalTrafficLights(Ts &&... xs) -> decltype(auto) + static auto sendCooperateCommand(Ts &&... xs) -> decltype(auto) { - return core->getConventionalTrafficLights(std::forward(xs)...); + return asFieldOperatorApplication(core->getEgoName()) + .sendCooperateCommand(std::forward(xs)...); } + // TrafficLights - Conventional and V2I template - static auto getV2ITrafficLights(Ts &&... xs) -> decltype(auto) + static auto setConventionalTrafficLightsState(Ts &&... xs) -> decltype(auto) { - return core->getV2ITrafficLights(std::forward(xs)...); + return core->getConventionalTrafficLights()->setTrafficLightsState( + std::forward(xs)...); } template - static auto resetConventionalTrafficLightPublishRate(Ts &&... xs) -> decltype(auto) + static auto setConventionalTrafficLightConfidence(Ts &&... xs) -> decltype(auto) { - return core->resetConventionalTrafficLightPublishRate(std::forward(xs)...); + return core->getConventionalTrafficLights()->setTrafficLightsConfidence( + std::forward(xs)...); } template - static auto resetV2ITrafficLightPublishRate(Ts &&... xs) -> decltype(auto) + static auto getConventionalTrafficLightsComposedState(Ts &&... xs) -> decltype(auto) { - return core->resetV2ITrafficLightPublishRate(std::forward(xs)...); + return core->getConventionalTrafficLights()->getTrafficLightsComposedState( + std::forward(xs)...); } template - static auto sendCooperateCommand(Ts &&... xs) -> decltype(auto) + static auto compareConventionalTrafficLightsState(Ts &&... xs) -> decltype(auto) { - return asFieldOperatorApplication(core->getEgoName()) - .sendCooperateCommand(std::forward(xs)...); + return core->getConventionalTrafficLights()->compareTrafficLightsState( + std::forward(xs)...); } template - static auto setConventionalTrafficLightConfidence(Ts &&... xs) -> decltype(auto) + static auto resetConventionalTrafficLightPublishRate(Ts &&... xs) -> decltype(auto) + { + return core->getConventionalTrafficLights()->resetUpdate(std::forward(xs)...); + } + + template + static auto setV2ITrafficLightsState(Ts &&... xs) -> decltype(auto) + { + return core->getV2ITrafficLights()->setTrafficLightsState(std::forward(xs)...); + } + + template + static auto resetV2ITrafficLightPublishRate(Ts &&... xs) -> decltype(auto) { - return core->setConventionalTrafficLightConfidence(std::forward(xs)...); + return core->getV2ITrafficLights()->resetUpdate(std::forward(xs)...); } }; }; diff --git a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp index 5cdc099fd56..15445fae827 100644 --- a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp @@ -164,11 +164,8 @@ struct ApplyV2ITrafficSignalStateAction : public CustomCommand, [[fallthrough]]; case 2: - for (auto & traffic_light : - getV2ITrafficLights(boost::lexical_cast(parameters[0]))) { - traffic_light.get().clear(); - traffic_light.get().set(unquote(parameters.at(1))); - } + setV2ITrafficLightsState( + boost::lexical_cast(parameters[0]), unquote(parameters.at(1))); break; default: diff --git a/openscenario/openscenario_interpreter/src/syntax/traffic_signal_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/traffic_signal_condition.cpp index e97b8c87a03..0270ca1d671 100644 --- a/openscenario/openscenario_interpreter/src/syntax/traffic_signal_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/traffic_signal_condition.cpp @@ -38,28 +38,12 @@ auto TrafficSignalCondition::description() const -> String auto TrafficSignalCondition::evaluate() -> Object { - if (auto && traffic_lights = - getConventionalTrafficLights(boost::lexical_cast(name)); - state == "none") { + const auto lanelet_id = boost::lexical_cast(name); + current_state = getConventionalTrafficLightsComposedState(lanelet_id); + if (current_state.empty()) { current_state = "none"; - return asBoolean(std::all_of( - std::begin(traffic_lights), std::end(traffic_lights), - [](const traffic_simulator::TrafficLight & traffic_light) { return traffic_light.empty(); })); - } else { - std::stringstream ss; - std::string separator = ""; - for (traffic_simulator::TrafficLight & traffic_light : traffic_lights) { - ss << separator << traffic_light; - separator = "; "; - } - current_state = ss.str(); - - return asBoolean(std::all_of( - std::begin(traffic_lights), std::end(traffic_lights), - [this](const traffic_simulator::TrafficLight & traffic_light) { - return traffic_light.contains(state); - })); } + return asBoolean(compareConventionalTrafficLightsState(lanelet_id, state)); } } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state.cpp b/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state.cpp index 7b972e5322c..f4aa0a7d039 100644 --- a/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state.cpp @@ -28,11 +28,7 @@ TrafficSignalState::TrafficSignalState(const pugi::xml_node & node, Scope & scop auto TrafficSignalState::evaluate() const -> Object { - for (traffic_simulator::TrafficLight & traffic_light : getConventionalTrafficLights(id())) { - traffic_light.clear(); - traffic_light.set(state); - }; - + setConventionalTrafficLightsState(id(), state); return unspecified; } diff --git a/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state_action.cpp b/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state_action.cpp index 45e1f59b7a9..9571d8d6fce 100644 --- a/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/traffic_signal_state_action.cpp @@ -33,10 +33,7 @@ auto TrafficSignalStateAction::run() noexcept -> void {} auto TrafficSignalStateAction::start() const -> void { - for (traffic_simulator::TrafficLight & traffic_light : getConventionalTrafficLights(id())) { - traffic_light.clear(); - traffic_light.set(state); - }; + setConventionalTrafficLightsState(id(), state); } auto TrafficSignalStateAction::id() const -> std::int64_t diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 67d0059618c..09a3e029752 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include #include #include @@ -84,7 +84,7 @@ class ActionNode : public BT::ActionNodeBase BT::InputPort>("target_speed"), BT::InputPort>("hdmap_utils"), BT::InputPort>("canonicalized_entity_status"), - BT::InputPort>("traffic_light_manager"), + BT::InputPort>("traffic_lights"), BT::InputPort("request"), BT::OutputPort>("obstacle"), BT::OutputPort("waypoints"), @@ -112,7 +112,7 @@ class ActionNode : public BT::ActionNodeBase protected: traffic_simulator::behavior::Request request; std::shared_ptr hdmap_utils; - std::shared_ptr traffic_light_manager; + std::shared_ptr traffic_lights; std::shared_ptr canonicalized_entity_status; double current_time; double step_time; diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp index edc05eb5d44..86d925e8a49 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp @@ -67,7 +67,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) DEFINE_GETTER_SETTER(StepTime, double) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) // clang-format on diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp index 59351fe827c..8333e5ae027 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp @@ -71,7 +71,7 @@ class VehicleBehaviorTree : public BehaviorPluginBase DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) DEFINE_GETTER_SETTER(StepTime, double) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) // clang-format on diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 29fcb393d7d..1fbc756306c 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -54,9 +54,9 @@ auto ActionNode::getBlackBoardValues() -> void if (!getInput>("hdmap_utils", hdmap_utils)) { THROW_SIMULATION_ERROR("failed to get input hdmap_utils in ActionNode"); } - if (!getInput>( - "traffic_light_manager", traffic_light_manager)) { - THROW_SIMULATION_ERROR("failed to get input traffic_light_manager in ActionNode"); + if (!getInput>( + "traffic_lights", traffic_lights)) { + THROW_SIMULATION_ERROR("failed to get input traffic_lights in ActionNode"); } if (!getInput>( "canonicalized_entity_status", canonicalized_entity_status)) { @@ -199,28 +199,23 @@ auto ActionNode::getDistanceToTrafficLightStopLine( const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional { - const auto traffic_light_ids = hdmap_utils->getTrafficLightIdsOnPath(route_lanelets); - if (traffic_light_ids.empty()) { - return std::nullopt; - } - std::set collision_points = {}; - for (const auto id : traffic_light_ids) { - using Color = traffic_simulator::TrafficLight::Color; - using Status = traffic_simulator::TrafficLight::Status; - using Shape = traffic_simulator::TrafficLight::Shape; - if (auto && traffic_light = traffic_light_manager->getTrafficLight(id); - traffic_light.contains(Color::red, Status::solid_on, Shape::circle) or - traffic_light.contains(Color::yellow, Status::solid_on, Shape::circle)) { - const auto collision_point = hdmap_utils->getDistanceToTrafficLightStopLine(spline, id); - if (collision_point) { - collision_points.insert(collision_point.value()); + if (const auto traffic_light_ids = hdmap_utils->getTrafficLightIdsOnPath(route_lanelets); + !traffic_light_ids.empty()) { + std::set collision_points = {}; + for (const auto traffic_light_id : traffic_light_ids) { + if (traffic_lights->isRequiredStopTrafficLightState(traffic_light_id)) { + if ( + const auto collision_point = + hdmap_utils->getDistanceToTrafficLightStopLine(spline, traffic_light_id)) { + collision_points.insert(collision_point.value()); + } } } + if (!collision_points.empty()) { + return *collision_points.begin(); + } } - if (collision_points.empty()) { - return std::nullopt; - } - return *collision_points.begin(); + return std::nullopt; } auto ActionNode::getDistanceToStopLine( diff --git a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp index 482e3411ff7..1804932d741 100644 --- a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp +++ b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp @@ -55,7 +55,7 @@ public: \ DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, std::shared_ptr) + DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) // clang-format on diff --git a/simulation/do_nothing_plugin/src/plugin.cpp b/simulation/do_nothing_plugin/src/plugin.cpp index c47663ddd41..af44aebae02 100644 --- a/simulation/do_nothing_plugin/src/plugin.cpp +++ b/simulation/do_nothing_plugin/src/plugin.cpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include namespace entity_behavior { diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index 5c3eede4d60..a66a436c362 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -37,12 +37,14 @@ ament_auto_add_library(simple_sensor_simulator_component SHARED src/sensor_simulation/lidar/lidar_sensor.cpp src/sensor_simulation/imu/imu_sensor.cpp src/sensor_simulation/lidar/raycaster.cpp - src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp - src/sensor_simulation/occupancy_grid/occupancy_grid_builder.cpp src/sensor_simulation/occupancy_grid/grid_traversal.cpp + src/sensor_simulation/occupancy_grid/occupancy_grid_builder.cpp + src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp src/sensor_simulation/primitives/box.cpp src/sensor_simulation/primitives/primitive.cpp + src/sensor_simulation/primitives/primitive.cpp src/sensor_simulation/sensor_simulation.cpp + src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp src/simple_sensor_simulator.cpp src/vehicle_simulation/ego_entity_simulation.cpp src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc.cpp diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp index 03a1c7313a7..2672a1e2987 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 TIER IV, Inc. 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. diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index 20f274f3276..9e078dda845 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -95,24 +95,10 @@ class SensorSimulation auto attachPseudoTrafficLightsDetector( const double /*current_simulation_time*/, const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration, - rclcpp::Node & node, std::shared_ptr hdmap_utils) -> void + rclcpp::Node & node) -> void { - if (configuration.architecture_type() == "awf/universe") { - using Message = autoware_auto_perception_msgs::msg::TrafficSignalArray; - traffic_lights_detectors_.push_back(std::make_unique( - std::make_shared>( - "/perception/traffic_light_recognition/traffic_signals", &node, hdmap_utils))); - } else if (configuration.architecture_type() >= "awf/universe/20230906") { - using Message = autoware_perception_msgs::msg::TrafficSignalArray; - traffic_lights_detectors_.push_back(std::make_unique( - std::make_shared>( - "/perception/traffic_light_recognition/internal/traffic_signals", &node, hdmap_utils))); - } else { - std::stringstream ss; - ss << "Unexpected architecture_type " << std::quoted(configuration.architecture_type()) - << " given."; - throw std::runtime_error(ss.str()); - } + traffic_lights_detectors_.push_back(std::make_unique( + node, configuration.architecture_type())); } auto attachImuSensor( diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp index b9d4f9155df..a93fc1e9dd4 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp @@ -16,10 +16,9 @@ #define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_DETECTOR_HPP_ #include +#include #include #include -#include -#include namespace simple_sensor_simulator { @@ -32,12 +31,10 @@ namespace traffic_lights */ class TrafficLightsDetector { - const std::shared_ptr publisher_; - public: - explicit TrafficLightsDetector( - const std::shared_ptr & publisher) - : publisher_(publisher) + template + explicit TrafficLightsDetector(NodeType & node, const std::string & architecture_type) + : publisher_ptr_(makePublisher(node, architecture_type)) { } @@ -45,8 +42,38 @@ class TrafficLightsDetector const rclcpp::Time & current_ros_time, const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void { - publisher_->publish(current_ros_time, request); + publisher_ptr_->publish(current_ros_time, request); + } + +private: + template + auto makePublisher(NodeType & node, const std::string & architecture_type) + -> std::unique_ptr + { + /* + TrafficLightsDetector in SimpleSensorSimulator publishes using architecture-dependent topics: + "/perception/traffic_light_recognition/internal/traffic_signals" for >= "awf/universe/20230906" + "/perception/traffic_light_recognition/traffic_signals" for "awf/universe" + + V2ITrafficLights in TrafficSimulator publishes publishes using architecture-independent topics ("awf/universe..."): + "/v2x/traffic_signals" and "/perception/traffic_light_recognition/external/traffic_signals" + */ + if (architecture_type == "awf/universe") { + using Message = autoware_auto_perception_msgs::msg::TrafficSignalArray; + return std::make_unique>( + &node, "/perception/traffic_light_recognition/traffic_signals"); + } else if (architecture_type >= "awf/universe/20230906") { + using Message = autoware_perception_msgs::msg::TrafficSignalArray; + return std::make_unique>( + &node, "/perception/traffic_light_recognition/internal/traffic_signals"); + } else { + std::stringstream ss; + ss << "Unexpected architecture_type " << std::quoted(architecture_type) << " given."; + throw std::invalid_argument(ss.str()); + } } + + const std::unique_ptr publisher_ptr_; }; } // namespace traffic_lights } // namespace simple_sensor_simulator diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp new file mode 100644 index 00000000000..87ce6f2f61a --- /dev/null +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp @@ -0,0 +1,56 @@ +// Copyright 2015 TIER IV, Inc. 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. + +#ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ + +#include +#include +#include +#include + +namespace simple_sensor_simulator +{ +namespace traffic_lights +{ +class TrafficLightsPublisherBase +{ +public: + virtual auto publish( + const rclcpp::Time & current_ros_time, + const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void = 0; +}; + +template +class TrafficLightsPublisher : public TrafficLightsPublisherBase +{ +public: + template + explicit TrafficLightsPublisher(const NodeTypePointer & node, const std::string & topic_name) + : TrafficLightsPublisherBase(), + traffic_light_state_array_publisher_( + rclcpp::create_publisher(node, topic_name, rclcpp::QoS(10).transient_local())) + { + } + + auto publish( + const rclcpp::Time & current_ros_time, + const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void override; + +private: + const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; +}; +} // namespace traffic_lights +} // namespace simple_sensor_simulator +#endif // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp index 9a86e1a3731..6fbb73119fb 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 TIER IV, Inc. 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. diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp new file mode 100644 index 00000000000..36b680138c2 --- /dev/null +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/traffic_lights/traffic_lights_publisher.cpp @@ -0,0 +1,76 @@ +// Copyright 2015 TIER IV, Inc. 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 +#include +#include + +namespace simple_sensor_simulator +{ +namespace traffic_lights +{ +template <> +auto TrafficLightsPublisher::publish( + const rclcpp::Time & current_ros_time, + const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void +{ + using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal; + using TrafficLightBulbType = TrafficLightType::_lights_type::value_type; + + autoware_auto_perception_msgs::msg::TrafficSignalArray traffic_lights_message; + traffic_lights_message.header.frame_id = "camera_link"; // DIRTY HACK!!! + traffic_lights_message.header.stamp = current_ros_time; + for (const auto & traffic_light : request.states()) { + // NOT skip if the traffic light has no bulbs + TrafficLightType single_traffic_light_message; + single_traffic_light_message.map_primitive_id = traffic_light.id(); + for (const auto & bulb_status : traffic_light.traffic_light_status()) { + TrafficLightBulbType light_bulb_message; + simulation_interface::toMsg(bulb_status, light_bulb_message); + single_traffic_light_message.lights.push_back(light_bulb_message); + } + traffic_lights_message.signals.push_back(single_traffic_light_message); + } + traffic_light_state_array_publisher_->publish(traffic_lights_message); +} + +template <> +auto TrafficLightsPublisher::publish( + const rclcpp::Time & current_ros_time, + const simulation_api_schema::UpdateTrafficLightsRequest & request) const -> void +{ + using TrafficLightType = autoware_perception_msgs::msg::TrafficSignal; + using TrafficLightBulbType = TrafficLightType::_elements_type::value_type; + + autoware_perception_msgs::msg::TrafficSignalArray traffic_lights_message; + traffic_lights_message.stamp = current_ros_time; + for (const auto & traffic_light : request.states()) { + for (const auto & relation_id : traffic_light.relation_ids()) { + // skip if the traffic light has no bulbs + if (not traffic_light.traffic_light_status().empty()) { + TrafficLightType single_traffic_light_message; + single_traffic_light_message.traffic_signal_id = relation_id; + for (const auto & bulb_status : traffic_light.traffic_light_status()) { + TrafficLightBulbType light_bulb_message; + simulation_interface::toMsg(bulb_status, light_bulb_message); + single_traffic_light_message.elements.push_back(light_bulb_message); + } + traffic_lights_message.signals.push_back(single_traffic_light_message); + } + } + } + traffic_light_state_array_publisher_->publish(traffic_lights_message); +} +} // namespace traffic_lights +} // namespace simple_sensor_simulator diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index b762ac6453b..6ea8b184437 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -354,7 +354,7 @@ auto ScenarioSimulator::attachPseudoTrafficLightDetector( { auto response = simulation_api_schema::AttachPseudoTrafficLightDetectorResponse(); sensor_sim_.attachPseudoTrafficLightsDetector( - current_simulation_time_, req.configuration(), *this, hdmap_utils_); + current_simulation_time_, req.configuration(), *this); response.mutable_result()->set_success(true); return response; } diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 2413b83d86c..d0dae2b7703 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -12,6 +12,7 @@ autoware_auto_control_msgs autoware_auto_vehicle_msgs + autoware_auto_perception_msgs boost builtin_interfaces geometry_msgs diff --git a/simulation/simulation_interface/proto/simulation_api_schema.proto b/simulation/simulation_interface/proto/simulation_api_schema.proto index 03d1e0e6252..c123b9e1dc6 100644 --- a/simulation/simulation_interface/proto/simulation_api_schema.proto +++ b/simulation/simulation_interface/proto/simulation_api_schema.proto @@ -322,6 +322,7 @@ message TrafficLight { message TrafficSignal { int32 id = 1; repeated TrafficLight traffic_light_status = 2; + repeated int32 relation_ids = 3; // TrafficLightRegulatoryElement ids based on TrafficLight id } /** diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index c7e9c3e212a..638ddd73fcf 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -45,17 +45,18 @@ ament_auto_add_library(traffic_simulator SHARED src/entity/vehicle_entity.cpp src/hdmap_utils/hdmap_utils.cpp src/helper/helper.cpp - src/job/job.cpp src/job/job_list.cpp + src/job/job.cpp src/simulation_clock/simulation_clock.cpp - src/traffic/traffic_controller.cpp - src/traffic/traffic_sink.cpp - src/traffic/traffic_source.cpp src/traffic_lights/configurable_rate_updater.cpp - src/traffic_lights/traffic_light.cpp - src/traffic_lights/traffic_light_manager.cpp src/traffic_lights/traffic_light_marker_publisher.cpp src/traffic_lights/traffic_light_publisher.cpp + src/traffic_lights/traffic_light.cpp + src/traffic_lights/traffic_lights_base.cpp + src/traffic_lights/traffic_lights.cpp + src/traffic/traffic_controller.cpp + src/traffic/traffic_sink.cpp + src/traffic/traffic_source.cpp src/utils/distance.cpp src/utils/pose.cpp ) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 28014db05be..1d5f44795fa 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -69,6 +70,9 @@ class API rclcpp::node_interfaces::get_node_parameters_interface(std::forward(node))), entity_manager_ptr_( std::make_shared(node, configuration, node_parameters_)), + traffic_lights_ptr_(std::make_shared( + node, entity_manager_ptr_->getHdmapUtils(), + getROS2Parameter("architecture_type", "awf/universe"))), traffic_controller_ptr_(std::make_shared( entity_manager_ptr_->getHdmapUtils(), [this]() { return API::getEntityNames(); }, [this](const auto & entity_name) { @@ -102,6 +106,7 @@ class API zeromq_client_( simulation_interface::protocol, configuration.simulator_host, getZMQSocketPort(*node)) { + entity_manager_ptr_->setTrafficLights(traffic_lights_ptr_); setVerbose(configuration.verbose); if (not configuration.standalone_mode) { @@ -335,6 +340,13 @@ class API const bool allow_spawn_outside_lane = false, const bool require_footprint_fitting = false, const bool random_orientation = false, std::optional random_seed = std::nullopt) -> void; + auto getV2ITrafficLights() { return traffic_lights_ptr_->getV2ITrafficLights(); } + + auto getConventionalTrafficLights() + { + return traffic_lights_ptr_->getConventionalTrafficLights(); + } + auto getEntity(const std::string & name) const -> std::shared_ptr; // clang-format off @@ -360,8 +372,6 @@ class API FORWARD_TO_ENTITY_MANAGER(entityExists); FORWARD_TO_ENTITY_MANAGER(getBehaviorParameter); FORWARD_TO_ENTITY_MANAGER(getBoundingBox); - FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLight); - FORWARD_TO_ENTITY_MANAGER(getConventionalTrafficLights); FORWARD_TO_ENTITY_MANAGER(getCurrentAccel); FORWARD_TO_ENTITY_MANAGER(getCurrentAction); FORWARD_TO_ENTITY_MANAGER(getCurrentTwist); @@ -373,13 +383,11 @@ class API FORWARD_TO_ENTITY_MANAGER(getLinearJerk); FORWARD_TO_ENTITY_MANAGER(getStandStillDuration); FORWARD_TO_ENTITY_MANAGER(getTraveledDistance); - FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight); - FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); - FORWARD_TO_ENTITY_MANAGER(reachPosition); FORWARD_TO_ENTITY_MANAGER(isEgoSpawned); FORWARD_TO_ENTITY_MANAGER(isInLanelet); FORWARD_TO_ENTITY_MANAGER(isNpcLogicStarted); FORWARD_TO_ENTITY_MANAGER(laneMatchingSucceed); + FORWARD_TO_ENTITY_MANAGER(reachPosition); FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition); FORWARD_TO_ENTITY_MANAGER(requestAssignRoute); FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory); @@ -388,13 +396,10 @@ class API FORWARD_TO_ENTITY_MANAGER(requestWalkStraight); FORWARD_TO_ENTITY_MANAGER(requestClearRoute); FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin); - FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate); - FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate); FORWARD_TO_ENTITY_MANAGER(setAcceleration); FORWARD_TO_ENTITY_MANAGER(setAccelerationLimit); FORWARD_TO_ENTITY_MANAGER(setAccelerationRateLimit); FORWARD_TO_ENTITY_MANAGER(setBehaviorParameter); - FORWARD_TO_ENTITY_MANAGER(setConventionalTrafficLightConfidence); FORWARD_TO_ENTITY_MANAGER(setDecelerationLimit); FORWARD_TO_ENTITY_MANAGER(setDecelerationRateLimit); FORWARD_TO_ENTITY_MANAGER(setLinearVelocity); @@ -421,6 +426,8 @@ class API const std::shared_ptr entity_manager_ptr_; + const std::shared_ptr traffic_lights_ptr_; + const std::shared_ptr traffic_controller_ptr_; const rclcpp::Publisher::SharedPtr clock_pub_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index 0d4847f7d5c..f4162f3e9b0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include @@ -71,7 +71,7 @@ class BehaviorPluginBase DEFINE_GETTER_SETTER(RouteLanelets, "route_lanelets", lanelet::Ids) DEFINE_GETTER_SETTER(StepTime, "step_time", double) DEFINE_GETTER_SETTER(TargetSpeed, "target_speed", std::optional) - DEFINE_GETTER_SETTER(TrafficLightManager, "traffic_light_manager", std::shared_ptr) + DEFINE_GETTER_SETTER(TrafficLights, "traffic_lights", std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, "vehicle_parameters", traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, "waypoints", traffic_simulator_msgs::msg::WaypointsArray) // clang-format on diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index a3cda521e26..5d362585dfb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include #include @@ -222,8 +222,8 @@ class EntityBase virtual auto setLinearVelocity(const double linear_velocity) -> void; - virtual void setTrafficLightManager( - const std::shared_ptr & traffic_light_manager); + virtual void setTrafficLights( + const std::shared_ptr & traffic_lights); virtual auto activateOutOfRangeJob( const double min_velocity, const double max_velocity, const double min_acceleration, @@ -268,7 +268,7 @@ class EntityBase CanonicalizedEntityStatus status_before_update_; std::shared_ptr hdmap_utils_ptr_; - std::shared_ptr traffic_light_manager_; + std::shared_ptr traffic_lights_; double stand_still_duration_ = 0.0; double traveled_distance_ = 0.0; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 70ce1b78f40..47c98cd586c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -98,18 +99,20 @@ class EntityManager MarkerArray markers_raw_; - const std::shared_ptr conventional_traffic_light_manager_ptr_; - const std::shared_ptr - conventional_traffic_light_marker_publisher_ptr_; - const std::shared_ptr - conventional_backward_compatible_traffic_light_publisher_ptr_; - const std::shared_ptr v2i_traffic_light_manager_ptr_; - const std::shared_ptr v2i_traffic_light_marker_publisher_ptr_; - const std::shared_ptr v2i_traffic_light_legacy_topic_publisher_ptr_; - const std::shared_ptr v2i_traffic_light_publisher_ptr_; - ConfigurableRateUpdater v2i_traffic_light_updater_, conventional_traffic_light_updater_; + std::shared_ptr traffic_lights_ptr_; public: + /** + * This function is necessary because the TrafficLights object is created after the EntityManager, + * so it cannot be assigned during the call of the EntityManager constructor. + * TrafficLights cannot be created before the EntityManager due to the dependency on HdMapUtils. + */ + auto setTrafficLights( + const std::shared_ptr & traffic_lights_ptr) -> void + { + traffic_lights_ptr_ = traffic_lights_ptr; + } + template auto getOrigin(Node & node) const { @@ -128,22 +131,6 @@ class EntityManager return origin; } - template - auto makeV2ITrafficLightPublisher(Ts &&... xs) -> std::shared_ptr - { - if (const auto architecture_type = - getParameter(node_parameters_, "architecture_type", "awf/universe"); - architecture_type.find("awf/universe") != std::string::npos) { - return std::make_shared< - TrafficLightPublisher>( - std::forward(xs)...); - } else { - throw common::SemanticError( - "Unexpected architecture_type ", std::quoted(architecture_type), - " given for V2I traffic lights simulation."); - } - } - template > explicit EntityManager( NodeT && node, const Configuration & configuration, @@ -163,36 +150,7 @@ class EntityManager rclcpp::PublisherOptionsWithAllocator())), hdmap_utils_ptr_(std::make_shared( configuration.lanelet2_map_path(), getOrigin(*node))), - markers_raw_(hdmap_utils_ptr_->generateMarker()), - conventional_traffic_light_manager_ptr_( - std::make_shared(hdmap_utils_ptr_)), - conventional_traffic_light_marker_publisher_ptr_( - std::make_shared(conventional_traffic_light_manager_ptr_, node)), - conventional_backward_compatible_traffic_light_publisher_ptr_( - std::make_shared>( - "/simulation/traffic_lights", node, hdmap_utils_ptr_)), - v2i_traffic_light_manager_ptr_(std::make_shared(hdmap_utils_ptr_)), - v2i_traffic_light_marker_publisher_ptr_( - std::make_shared(v2i_traffic_light_manager_ptr_, node)), - v2i_traffic_light_legacy_topic_publisher_ptr_( - makeV2ITrafficLightPublisher("/v2x/traffic_signals", node, hdmap_utils_ptr_)), - v2i_traffic_light_publisher_ptr_(makeV2ITrafficLightPublisher( - "/perception/traffic_light_recognition/external/traffic_signals", node, hdmap_utils_ptr_)), - v2i_traffic_light_updater_( - node, - [this]() { - v2i_traffic_light_marker_publisher_ptr_->publish(); - v2i_traffic_light_publisher_ptr_->publish( - clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); - v2i_traffic_light_legacy_topic_publisher_ptr_->publish( - clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); - }), - conventional_traffic_light_updater_(node, [this]() { - conventional_traffic_light_marker_publisher_ptr_->publish(); - conventional_backward_compatible_traffic_light_publisher_ptr_->publish( - clock_ptr_->now(), - conventional_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); - }) + markers_raw_(hdmap_utils_ptr_->generateMarker()) { updateHdmapMarker(); } @@ -200,47 +158,6 @@ class EntityManager ~EntityManager() = default; public: -#define FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(NAME) \ - template \ - decltype(auto) getConventional##NAME(Ts &&... xs) const \ - { \ - return conventional_traffic_light_manager_ptr_->get##NAME(xs...); \ - } \ - static_assert(true, ""); \ - template \ - decltype(auto) getV2I##NAME(Ts &&... xs) const \ - { \ - return v2i_traffic_light_manager_ptr_->get##NAME(xs...); \ - } \ - static_assert(true, "") - - FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(TrafficLights); - FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER(TrafficLight); - -#undef FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER - - auto generateUpdateRequestForConventionalTrafficLights() - { - return conventional_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest(); - } - - auto resetConventionalTrafficLightPublishRate(double rate) -> void - { - conventional_traffic_light_updater_.resetUpdateRate(rate); - } - - auto resetV2ITrafficLightPublishRate(double rate) -> void - { - v2i_traffic_light_updater_.resetUpdateRate(rate); - } - - auto setConventionalTrafficLightConfidence(lanelet::Id id, double confidence) -> void - { - for (auto & traffic_light : conventional_traffic_light_manager_ptr_->getTrafficLights(id)) { - traffic_light.get().confidence = confidence; - } - } - // clang-format off #define FORWARD_TO_ENTITY(IDENTIFIER, ...) \ /*! \ @@ -304,8 +221,6 @@ class EntityManager visualization_msgs::msg::MarkerArray makeDebugMarker() const; - bool trafficLightsChanged(); - auto updateNpcLogic(const std::string & name, const double current_time, const double step_time) -> const CanonicalizedEntityStatus &; @@ -471,7 +386,7 @@ class EntityManager std::forward(xs)...)); success) { // FIXME: this ignores V2I traffic lights - iter->second->setTrafficLightManager(conventional_traffic_light_manager_ptr_); + iter->second->setTrafficLights(traffic_lights_ptr_->getConventionalTrafficLights()); return success; } else { THROW_SEMANTIC_ERROR("Entity ", std::quoted(name), " is already exists."); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp index ce36aa648b0..4a142aed640 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp @@ -78,8 +78,7 @@ class PedestrianEntity : public EntityBase void cancelRequest() override; - void setTrafficLightManager( - const std::shared_ptr & ptr) override; + void setTrafficLights(const std::shared_ptr & ptr) override; auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp index d3b6b53567c..7c4829d7224 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -123,8 +123,7 @@ class VehicleEntity : public EntityBase void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override; - void setTrafficLightManager( - const std::shared_ptr &) override; + void setTrafficLights(const std::shared_ptr &) override; const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp index 695511cebfc..8833fd138b3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/configurable_rate_updater.hpp @@ -22,28 +22,30 @@ namespace traffic_simulator { class ConfigurableRateUpdater { - rclcpp::TimerBase::SharedPtr timer_ = nullptr; - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; - const rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_interface_; - double update_rate_ = 0.0; - const std::function thunk_; - const rclcpp::Clock::SharedPtr clock_ptr_; - public: template ConfigurableRateUpdater(const NodePointer & node, std::function thunk) : node_base_interface_(node->get_node_base_interface()), node_timers_interface_(node->get_node_timers_interface()), - thunk_(thunk), - clock_ptr_(node->get_clock()) + clock_ptr_(node->get_clock()), + thunk_(std::move(thunk)) { } - auto createTimer(double update_rate) -> void; + auto startTimer(const double update_rate) -> void; - auto resetUpdateRate(double update_rate) -> void; + auto resetTimer(const double update_rate) -> void; auto getUpdateRate() const -> double { return update_rate_; } + +private: + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; + const rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_interface_; + const rclcpp::Clock::SharedPtr clock_ptr_; + + rclcpp::TimerBase::SharedPtr timer_{nullptr}; + const std::function thunk_; + double update_rate_ = 0.0; }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp index 88fed84ff8a..e6a92f122a0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light.hpp @@ -15,6 +15,8 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_HPP_ #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_HPP_ +#include + #include #include #include @@ -25,9 +27,9 @@ #include #include #include -#include #include #include +#include #include #include #include @@ -252,61 +254,79 @@ struct TrafficLight explicit operator simulation_api_schema::TrafficLight() const { - auto color = [this]() { + const auto color = [this]() { + auto color_message = simulation_api_schema::TrafficLight_Color_UNKNOWN_COLOR; switch (std::get(value).value) { case Color::green: - return simulation_api_schema::TrafficLight_Color_GREEN; + color_message = simulation_api_schema::TrafficLight_Color_GREEN; + break; case Color::yellow: - return simulation_api_schema::TrafficLight_Color_AMBER; + color_message = simulation_api_schema::TrafficLight_Color_AMBER; + break; case Color::red: - return simulation_api_schema::TrafficLight_Color_RED; + color_message = simulation_api_schema::TrafficLight_Color_RED; + break; case Color::white: - return simulation_api_schema::TrafficLight_Color_WHITE; - default: - throw common::SyntaxError(std::get(value), " is not supported color."); + color_message = simulation_api_schema::TrafficLight_Color_WHITE; + break; } + return color_message; }; - auto status = [this]() { + const auto status = [this]() { + auto status_message = simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; switch (std::get(value).value) { case Status::solid_on: - return simulation_api_schema::TrafficLight_Status_SOLID_ON; + status_message = simulation_api_schema::TrafficLight_Status_SOLID_ON; + break; case Status::solid_off: - return simulation_api_schema::TrafficLight_Status_SOLID_OFF; + status_message = simulation_api_schema::TrafficLight_Status_SOLID_OFF; + break; case Status::flashing: - return simulation_api_schema::TrafficLight_Status_FLASHING; + status_message = simulation_api_schema::TrafficLight_Status_FLASHING; + break; case Status::unknown: - return simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; - default: - throw common::SyntaxError(std::get(value), " is not supported as a status."); + status_message = simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS; + break; } + return status_message; }; - auto shape = [this]() { + const auto shape = [this]() { + auto shape_message = simulation_api_schema::TrafficLight_Shape_UNKNOWN_SHAPE; switch (std::get(value).value) { case Shape::circle: - return simulation_api_schema::TrafficLight_Shape_CIRCLE; + shape_message = simulation_api_schema::TrafficLight_Shape_CIRCLE; + break; case Shape::cross: - return simulation_api_schema::TrafficLight_Shape_CROSS; + shape_message = simulation_api_schema::TrafficLight_Shape_CROSS; + break; case Shape::left: - return simulation_api_schema::TrafficLight_Shape_LEFT_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_LEFT_ARROW; + break; case Shape::down: - return simulation_api_schema::TrafficLight_Shape_DOWN_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_DOWN_ARROW; + break; case Shape::up: - return simulation_api_schema::TrafficLight_Shape_UP_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_UP_ARROW; + break; case Shape::right: - return simulation_api_schema::TrafficLight_Shape_RIGHT_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_RIGHT_ARROW; + break; case Shape::lower_left: - return simulation_api_schema::TrafficLight_Shape_DOWN_LEFT_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_DOWN_LEFT_ARROW; + break; case Shape::lower_right: - return simulation_api_schema::TrafficLight_Shape_DOWN_RIGHT_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_DOWN_RIGHT_ARROW; + break; case Shape::upper_left: - return simulation_api_schema::TrafficLight_Shape_UP_LEFT_ARROW; + shape_message = simulation_api_schema::TrafficLight_Shape_UP_LEFT_ARROW; + break; case Shape::upper_right: - return simulation_api_schema::TrafficLight_Shape_UP_RIGHT_ARROW; - default: - throw common::SyntaxError(std::get(value), " is not supported as a shape."); + shape_message = simulation_api_schema::TrafficLight_Shape_UP_RIGHT_ARROW; + break; } + return shape_message; }; simulation_api_schema::TrafficLight traffic_light_bulb_proto; @@ -318,18 +338,108 @@ struct TrafficLight return traffic_light_bulb_proto; } + + explicit operator traffic_simulator_msgs::msg::TrafficLightBulbV1() const + { + const auto color = [this] { + auto color_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; + switch (std::get(value).value) { + case Color::green: + color_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::GREEN; + break; + case Color::yellow: + color_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::AMBER; + break; + case Color::red: + color_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::RED; + break; + case Color::white: + color_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::WHITE; + break; + } + return color_message; + }; + + const auto status = [this] { + auto status_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; + switch (std::get(value).value) { + case Status::solid_on: + status_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::SOLID_ON; + break; + case Status::solid_off: + status_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::SOLID_OFF; + break; + case Status::flashing: + status_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::FLASHING; + break; + case Status::unknown: + status_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; + break; + } + return status_message; + }; + + const auto shape = [this] { + auto shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UNKNOWN; + switch (std::get(value).value) { + case Shape::circle: + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::CIRCLE; + break; + case Shape::cross: + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::CROSS; + break; + case Shape::left: + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::LEFT_ARROW; + break; + case Shape::down: + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_ARROW; + break; + case Shape::up: + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_ARROW; + break; + case Shape::right: + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::RIGHT_ARROW; + break; + case Shape::lower_left: + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_LEFT_ARROW; + break; + case Shape::lower_right: + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::DOWN_RIGHT_ARROW; + break; + case Shape::upper_left: + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_LEFT_ARROW; + break; + case Shape::upper_right: + shape_message = traffic_simulator_msgs::msg::TrafficLightBulbV1::UP_RIGHT_ARROW; + break; + } + return shape_message; + }; + + traffic_simulator_msgs::msg::TrafficLightBulbV1 msg; + msg.color = color(); + msg.status = status(); + msg.shape = shape(); + // NOTE: confidence will be overwritten + msg.confidence = 1.0; + // NOTE: unused data member 'enum_revision' for + // traffic_simulator_msgs::msg::TrafficLightBulbV1 + return msg; + } }; + explicit TrafficLight(const lanelet::Id, const hdmap_utils::HdMapUtils &); + const lanelet::Id way_id; + const lanelet::Ids regulatory_elements_ids; + double confidence = 1.0; std::set bulbs; const std::map> positions; - explicit TrafficLight(const lanelet::Id, hdmap_utils::HdMapUtils &); - auto clear() { bulbs.clear(); } auto contains(const Bulb & bulb) const { return bulbs.find(bulb) != std::end(bulbs); } @@ -397,6 +507,18 @@ struct TrafficLight } return traffic_signal_proto; } + + explicit operator traffic_simulator_msgs::msg::TrafficLightV1() const + { + traffic_simulator_msgs::msg::TrafficLightV1 traffic_signal; + traffic_signal.lanelet_way_id = way_id; + for (const auto & bulb : bulbs) { + auto traffic_light_bulb = static_cast(bulb); + traffic_light_bulb.confidence = confidence; + traffic_signal.traffic_light_bulbs.push_back(traffic_light_bulb); + } + return traffic_signal; + } }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp deleted file mode 100644 index c43e9e940a6..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2015 TIER IV, Inc. 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. - -#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MANAGER_BASE_HPP_ -#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MANAGER_BASE_HPP_ - -#include -#include -#include -#include -#include // std::out_of_range -#include -#include -#include -#include -#include -#include -#include - -namespace traffic_simulator -{ -class TrafficLightManager -{ -protected: - using TrafficLightMap = std::unordered_map; - - TrafficLightMap traffic_lights_; - - const std::shared_ptr hdmap_; - -public: - explicit TrafficLightManager(const std::shared_ptr & hdmap); - - auto getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight &; - - auto getTrafficLightIds() const -> const lanelet::Ids; - - auto getTrafficLights() const -> const TrafficLightMap &; - - auto getTrafficLights() -> TrafficLightMap &; - - auto getTrafficLights(const lanelet::Id lanelet_id) - -> std::vector>; - - auto hasAnyLightChanged() -> bool; - - auto generateUpdateTrafficLightsRequest() -> simulation_api_schema::UpdateTrafficLightsRequest; -}; -} // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MANAGER_BASE_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp index 9e2524c7077..7cdcedb61b8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp @@ -15,35 +15,33 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP -#include +#include +#include namespace traffic_simulator { class TrafficLightMarkerPublisher { - const rclcpp::Publisher::SharedPtr marker_pub_; - const std::string map_frame_; - const rclcpp::Clock::SharedPtr clock_ptr_; - const std::shared_ptr traffic_light_manager_; - - auto deleteAllMarkers() const -> void; - auto drawMarkers() const -> void; - public: - template + template explicit TrafficLightMarkerPublisher( - const std::shared_ptr & traffic_light_manager, const NodePointer & node, - const std::string & map_frame = "map") - : marker_pub_(rclcpp::create_publisher( - node, "traffic_light/marker", rclcpp::QoS(1).transient_local())), - map_frame_(map_frame), - clock_ptr_(node->get_clock()), - traffic_light_manager_(traffic_light_manager) + const NodeTypePointer & node_ptr, const std::string & frame = "map") + : frame_(frame), + clock_ptr_(node_ptr->get_clock()), + publisher_(rclcpp::create_publisher( + node_ptr, "traffic_light/marker", rclcpp::QoS(1).transient_local())) { } - auto publish() -> void; -}; + auto deleteMarkers() const -> void; + auto drawMarkers(const std::unordered_map & traffic_lights_map) const + -> void; + +private: + const std::string frame_; + const rclcpp::Clock::SharedPtr clock_ptr_; + const rclcpp::Publisher::SharedPtr publisher_; +}; } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_MARKER_PUBLISHER_HPP diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp index d292f01582c..618dda3a59a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp @@ -17,42 +17,44 @@ #include #include -#include #include #include +#include +#include namespace traffic_simulator { class TrafficLightPublisherBase { public: - virtual auto publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void = 0; + virtual auto publish(const TrafficLightsBase & traffic_lights) const -> void = 0; + virtual ~TrafficLightPublisherBase() = default; }; -template +template class TrafficLightPublisher : public TrafficLightPublisherBase { - const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; - - const std::shared_ptr hdmap_utils_; - public: - template + template explicit TrafficLightPublisher( - const std::string & topic_name, const NodePointer & node, - const std::shared_ptr & hdmap_utils = nullptr) + const NodeTypePointer & node_ptr, const std::string & topic_name, + const std::string & frame = "camera_link") : TrafficLightPublisherBase(), - traffic_light_state_array_publisher_( - rclcpp::create_publisher(node, topic_name, rclcpp::QoS(10).transient_local())), - hdmap_utils_(hdmap_utils) + frame_(frame), + clock_ptr_(node_ptr->get_clock()), + traffic_light_state_array_publisher_(rclcpp::create_publisher( + node_ptr, topic_name, rclcpp::QoS(10).transient_local())) { } - auto publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void override; + ~TrafficLightPublisher() override = default; + + auto publish(const TrafficLightsBase & traffic_lights) const -> void override; + +private: + const std::string frame_; + const rclcpp::Clock::SharedPtr clock_ptr_; + const typename rclcpp::Publisher::SharedPtr traffic_light_state_array_publisher_; }; } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp new file mode 100644 index 00000000000..e2ae7cbd4b7 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights.hpp @@ -0,0 +1,138 @@ +// Copyright 2015 TIER IV, Inc. 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. + +#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_HPP_ +#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_HPP_ + +#include +#include +#include +#include + +namespace traffic_simulator +{ +class ConventionalTrafficLights : public TrafficLightsBase +{ +public: + template + explicit ConventionalTrafficLights( + const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils) + : TrafficLightsBase(node_ptr, hdmap_utils), + backward_compatible_publisher_ptr_( + std::make_unique>( + node_ptr, "/simulation/traffic_lights")) + { + } + + ~ConventionalTrafficLights() override = default; + +private: + auto update() const -> void + { + backward_compatible_publisher_ptr_->publish(*this); + if (isAnyTrafficLightChanged()) { + marker_publisher_ptr_->deleteMarkers(); + } + marker_publisher_ptr_->drawMarkers(traffic_lights_map_); + } + + const std::unique_ptr backward_compatible_publisher_ptr_; +}; + +class V2ITrafficLights : public TrafficLightsBase +{ +public: + template + explicit V2ITrafficLights( + const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils, + const std::string & architecture_type) + : TrafficLightsBase(node_ptr, hdmap_utils), + publisher_ptr_(makePublisher( + node_ptr, architecture_type, + "/perception/traffic_light_recognition/external/traffic_signals")), + legacy_topic_publisher_ptr_(makePublisher(node_ptr, architecture_type, "/v2x/traffic_signals")) + { + } + + ~V2ITrafficLights() override = default; + +private: + auto update() const -> void override + { + publisher_ptr_->publish(*this); + legacy_topic_publisher_ptr_->publish(*this); + if (isAnyTrafficLightChanged()) { + marker_publisher_ptr_->deleteMarkers(); + } + marker_publisher_ptr_->drawMarkers(traffic_lights_map_); + } + + template + auto makePublisher( + const NodeTypePointer & node_ptr, const std::string & architecture_type, + const std::string & topic_name) -> std::unique_ptr + { + /* + V2ITrafficLights in TrafficSimulator publishes publishes using architecture-independent topics ("awf/universe..."): + "/v2x/traffic_signals" and "/perception/traffic_light_recognition/external/traffic_signals" + + TrafficLightsDetector in SimpleSensorSimulator publishes using architecture-dependent topics: + "/perception/traffic_light_recognition/internal/traffic_signals" for >= "awf/universe/20230906" + "/perception/traffic_light_recognition/traffic_signals" for "awf/universe" + */ + if (architecture_type.find("awf/universe") != std::string::npos) { + return std::make_unique< + TrafficLightPublisher>( + node_ptr, topic_name); + } else { + throw common::SemanticError( + "Unexpected architecture_type ", std::quoted(architecture_type), + " given for V2I traffic lights simulation."); + } + } + + const std::unique_ptr publisher_ptr_; + const std::unique_ptr legacy_topic_publisher_ptr_; +}; + +class TrafficLights +{ +public: + template + explicit TrafficLights( + const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils, + const std::string & architecture_type) + : conventional_traffic_lights_( + std::make_shared(node_ptr, hdmap_utils)), + v2i_traffic_lights_( + std::make_shared(node_ptr, hdmap_utils, architecture_type)) + { + } + + auto isAnyTrafficLightChanged() -> bool; + + auto startTrafficLightsUpdate( + const double conventional_traffic_light_update_rate, + const double v2i_traffic_lights_update_rate) -> void; + + auto getConventionalTrafficLights() const -> std::shared_ptr; + + auto getV2ITrafficLights() const -> std::shared_ptr; + +private: + const std::shared_ptr conventional_traffic_lights_; + const std::shared_ptr v2i_traffic_lights_; +}; +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp new file mode 100644 index 00000000000..8220cf8d6a5 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_lights_base.hpp @@ -0,0 +1,100 @@ +// Copyright 2015 TIER IV, Inc. 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. + +#ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_ +#define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +/* + TrafficLightsBase class is designed in such a way that while trying to perform an operation + on a TrafficLight (add, set, etc.) that is not added to traffic_light_map_, + it adds the traffic light first and then performs the operation, so that the methods + here cannot be tagged with the "const" specifier +*/ +class TrafficLightsBase +{ +public: + template + explicit TrafficLightsBase( + const NodeTypePointer & node_ptr, const std::shared_ptr & hdmap_utils) + : hdmap_utils_(hdmap_utils), + clock_ptr_(node_ptr->get_clock()), + marker_publisher_ptr_(std::make_unique(node_ptr)), + rate_updater_(node_ptr, [this]() { update(); }) + { + } + + virtual ~TrafficLightsBase() = default; + + // update + auto startUpdate(const double update_rate) -> void; + + auto resetUpdate(const double update_rate) -> void; + + // checks, setters, getters + auto isAnyTrafficLightChanged() const -> bool; + + auto isRequiredStopTrafficLightState(const lanelet::Id traffic_light_id) -> bool; + + auto compareTrafficLightsState(const lanelet::Id lanelet_id, const std::string & state) -> bool; + + auto setTrafficLightsColor( + const lanelet::Id lanelet_id, const traffic_simulator::TrafficLight::Color & color) -> void; + + auto setTrafficLightsState(const lanelet::Id lanelet_id, const std::string & state) -> void; + + auto setTrafficLightsConfidence(const lanelet::Id lanelet_id, const double confidence) -> void; + + auto getTrafficLightsComposedState(const lanelet::Id lanelet_id) -> std::string; + + auto generateUpdateTrafficLightsRequest() const + -> simulation_api_schema::UpdateTrafficLightsRequest; + + auto generateTrafficSimulatorV1Msg() const -> traffic_simulator_msgs::msg::TrafficLightArrayV1; + +protected: + virtual auto update() const -> void = 0; + + auto isTrafficLightAdded(const lanelet::Id traffic_light_id) const -> bool; + + auto addTrafficLight(const lanelet::Id traffic_light_id) -> void; + + auto getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight &; + + auto getTrafficLights(const lanelet::Id lanelet_id) + -> std::vector>; + + const std::shared_ptr hdmap_utils_; + const rclcpp::Clock::SharedPtr clock_ptr_; + + std::unordered_map traffic_lights_map_; + const std::unique_ptr marker_publisher_ptr_; + ConfigurableRateUpdater rate_updater_; +}; +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_BASE_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index f269ecdc0f9..80b1a287e88 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 TIER IV, Inc. 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. diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp index 5114d9997af..c24f848e462 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/node_parameters.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 TIER IV, Inc. 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. diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 5a0f6cae523..37428420fde 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 TIER IV, Inc. 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. diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 5018b3d7659..1f24e201b3c 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -289,9 +289,10 @@ bool API::updateTimeInSim() bool API::updateTrafficLightsInSim() { - if (entity_manager_ptr_->trafficLightsChanged()) { - auto req = entity_manager_ptr_->generateUpdateRequestForConventionalTrafficLights(); - return zeromq_client_.call(req).result().success(); + if (traffic_lights_ptr_->isAnyTrafficLightChanged()) { + auto request = + traffic_lights_ptr_->getConventionalTrafficLights()->generateUpdateTrafficLightsRequest(); + return zeromq_client_.call(request).result().success(); } /// @todo handle response return simulation_api_schema::UpdateTrafficLightsResponse().result().success(); diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 0143237b88c..5537f99c48e 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -568,10 +568,10 @@ auto EntityBase::setLinearAcceleration(const double linear_acceleration) -> void status_->setLinearAcceleration(linear_acceleration); } -void EntityBase::setTrafficLightManager( - const std::shared_ptr & traffic_light_manager) +void EntityBase::setTrafficLights( + const std::shared_ptr & traffic_lights) { - traffic_light_manager_ = traffic_light_manager; + traffic_lights_ = traffic_lights; } auto EntityBase::setTwist(const geometry_msgs::msg::Twist & twist) -> void diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 7b730145ccd..4211cc33c93 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -334,12 +335,6 @@ auto EntityManager::getCurrentAction(const std::string & name) const -> std::str } } -bool EntityManager::trafficLightsChanged() -{ - return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or - v2i_traffic_light_manager_ptr_->hasAnyLightChanged(); -} - void EntityManager::setVerbose(const bool verbose) { configuration.verbose = verbose; @@ -374,9 +369,9 @@ void EntityManager::update(const double current_time, const double step_time) "EntityManager::update", configuration.verbose); setVerbose(configuration.verbose); if (npc_logic_started_) { - conventional_traffic_light_updater_.createTimer( - configuration.conventional_traffic_light_publish_rate); - v2i_traffic_light_updater_.createTimer(configuration.v2i_traffic_light_publish_rate); + traffic_lights_ptr_->startTrafficLightsUpdate( + configuration.conventional_traffic_light_publish_rate, + configuration.v2i_traffic_light_publish_rate); } std::unordered_map all_status; for (auto && [name, entity] : entities_) { diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index 061a3cbde76..b368b9d8839 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -167,11 +167,11 @@ auto PedestrianEntity::getEntityTypename() const -> const std::string & return result; } -void PedestrianEntity::setTrafficLightManager( - const std::shared_ptr & ptr) +void PedestrianEntity::setTrafficLights( + const std::shared_ptr & ptr) { - EntityBase::setTrafficLightManager(ptr); - behavior_plugin_ptr_->setTrafficLightManager(traffic_light_manager_); + EntityBase::setTrafficLights(ptr); + behavior_plugin_ptr_->setTrafficLights(traffic_lights_); } auto PedestrianEntity::getBehaviorParameter() const diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index 10d6d921237..738e961d53f 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -317,11 +317,11 @@ void VehicleEntity::setBehaviorParameter( behavior_plugin_ptr_->setBehaviorParameter(parameter); } -void VehicleEntity::setTrafficLightManager( - const std::shared_ptr & ptr) +void VehicleEntity::setTrafficLights( + const std::shared_ptr & ptr) { - EntityBase::setTrafficLightManager(ptr); - behavior_plugin_ptr_->setTrafficLightManager(traffic_light_manager_); + EntityBase::setTrafficLights(ptr); + behavior_plugin_ptr_->setTrafficLights(traffic_lights_); } } // namespace entity diff --git a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp index fd1f41e87a3..7858114ccb0 100644 --- a/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/configurable_rate_updater.cpp @@ -12,33 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include namespace traffic_simulator { -auto ConfigurableRateUpdater::createTimer(double update_rate) -> void +auto ConfigurableRateUpdater::startTimer(const double update_rate) -> void { if (!timer_) { update_rate_ = update_rate; - using namespace std::chrono_literals; + const auto period = std::chrono::duration(1.0 / update_rate_); timer_ = rclcpp::create_timer( - node_base_interface_, node_timers_interface_, clock_ptr_, 1s / update_rate_, - [this]() -> void { thunk_(); }); + node_base_interface_, node_timers_interface_, clock_ptr_, period, [this]() { thunk_(); }); } } -auto ConfigurableRateUpdater::resetUpdateRate(double update_rate) -> void +auto ConfigurableRateUpdater::resetTimer(const double update_rate) -> void { - if (update_rate_ != update_rate) { - update_rate_ = update_rate; - if (timer_ && not timer_->is_canceled()) { - timer_->cancel(); - } - - using namespace std::chrono_literals; - timer_ = rclcpp::create_timer( - node_base_interface_, node_timers_interface_, clock_ptr_, 1s / update_rate_, - [this]() -> void { thunk_(); }); + if (timer_ and not timer_->is_canceled()) { + timer_->cancel(); + timer_.reset(); } + startTimer(update_rate); } } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp index 4501ba7ae0d..f7cc4e8079a 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light.cpp @@ -12,14 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include #include -#include -#include -#include namespace traffic_simulator { @@ -166,13 +160,14 @@ auto operator<<(std::ostream & os, const TrafficLight::Bulb & bulb) -> std::ostr << std::get(bulb.value); } -TrafficLight::TrafficLight(const lanelet::Id lanelet_id, hdmap_utils::HdMapUtils & map_manager) +TrafficLight::TrafficLight( + const lanelet::Id lanelet_id, const hdmap_utils::HdMapUtils & hdmap_utils) : way_id([&]() { - if (map_manager.isTrafficLight(lanelet_id)) { + if (hdmap_utils.isTrafficLight(lanelet_id)) { return lanelet_id; } else { // lanelet::RoleName::Refers - if (auto traffic_light_members = map_manager.getTrafficLightRegulatoryElement(lanelet_id) + if (auto traffic_light_members = hdmap_utils.getTrafficLightRegulatoryElement(lanelet_id) ->getParameters("refers"); traffic_light_members.size() > 0) { // Note: If `lanelet_id` is a relation id, it is okay to use only one of the referred way ids. @@ -184,16 +179,17 @@ TrafficLight::TrafficLight(const lanelet::Id lanelet_id, hdmap_utils::HdMapUtils } } }()), + regulatory_elements_ids(hdmap_utils.getTrafficLightRegulatoryElementIDsFromTrafficLight(way_id)), positions{ std::make_pair( Bulb(Color::green, Status::solid_on, Shape::circle).hash(), - map_manager.getTrafficLightBulbPosition(way_id, "green")), + hdmap_utils.getTrafficLightBulbPosition(way_id, "green")), std::make_pair( Bulb(Color::yellow, Status::solid_on, Shape::circle).hash(), - map_manager.getTrafficLightBulbPosition(way_id, "yellow")), + hdmap_utils.getTrafficLightBulbPosition(way_id, "yellow")), std::make_pair( Bulb(Color::red, Status::solid_on, Shape::circle).hash(), - map_manager.getTrafficLightBulbPosition(way_id, "red")), + hdmap_utils.getTrafficLightBulbPosition(way_id, "red")), } { } diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp deleted file mode 100644 index fe983240de8..00000000000 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright 2015 TIER IV, Inc. 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 -#include -#include -#include -#include -#include -#include - -namespace traffic_simulator -{ -TrafficLightManager::TrafficLightManager(const std::shared_ptr & hdmap) -: hdmap_(hdmap) -{ -} - -auto TrafficLightManager::hasAnyLightChanged() -> bool -{ - return true; - // return std::any_of( - // std::begin(getTrafficLights()), std::end(getTrafficLights()), [](auto && id_and_traffic_light) { - // return id_and_traffic_light.second.colorChanged() or - // id_and_traffic_light.second.arrowChanged(); - // }); -} - -auto TrafficLightManager::getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight & -{ - if (auto iter = traffic_lights_.find(traffic_light_id); iter != std::end(traffic_lights_)) { - return iter->second; - } else { - traffic_lights_.emplace( - std::piecewise_construct, std::forward_as_tuple(traffic_light_id), - std::forward_as_tuple(traffic_light_id, *hdmap_)); - return traffic_lights_.at(traffic_light_id); - } -} - -auto TrafficLightManager::getTrafficLightIds() const -> const lanelet::Ids -{ - lanelet::Ids traffic_light_ids; - for (const auto & traffic_light_ : getTrafficLights()) { - traffic_light_ids.push_back(traffic_light_.first); - } - return traffic_light_ids; -} - -auto TrafficLightManager::getTrafficLights() const -> const TrafficLightMap & -{ - return traffic_lights_; -} - -auto TrafficLightManager::getTrafficLights() -> TrafficLightMap & { return traffic_lights_; } - -auto TrafficLightManager::getTrafficLights(const lanelet::Id lanelet_id) - -> std::vector> -{ - std::vector> traffic_lights; - - if (hdmap_->isTrafficLightRegulatoryElement(lanelet_id)) { - for (auto && traffic_light : - hdmap_->getTrafficLightRegulatoryElement(lanelet_id)->trafficLights()) { - traffic_lights.emplace_back(getTrafficLight(traffic_light.id())); - } - } else if (hdmap_->isTrafficLight(lanelet_id)) { - traffic_lights.emplace_back(getTrafficLight(lanelet_id)); - } else { - throw common::scenario_simulator_exception::Error( - "Given lanelet ID ", lanelet_id, " is neither a traffic light ID not a traffic relation ID."); - } - - return traffic_lights; -} - -auto TrafficLightManager::generateUpdateTrafficLightsRequest() - -> simulation_api_schema::UpdateTrafficLightsRequest -{ - simulation_api_schema::UpdateTrafficLightsRequest update_traffic_lights_request; - for (auto && [lanelet_id, traffic_light] : traffic_lights_) { - *update_traffic_lights_request.add_states() = - static_cast(traffic_light); - } - return update_traffic_lights_request; -} - -} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp index 3cf16db030f..e447f35d385 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_marker_publisher.cpp @@ -17,38 +17,22 @@ namespace traffic_simulator { -auto TrafficLightMarkerPublisher::deleteAllMarkers() const -> void -{ - visualization_msgs::msg::MarkerArray message; - { - visualization_msgs::msg::Marker marker; - marker.action = marker.DELETEALL; - message.markers.push_back(marker); - } - - marker_pub_->publish(message); -} - -auto TrafficLightMarkerPublisher::drawMarkers() const -> void +auto TrafficLightMarkerPublisher::deleteMarkers() const -> void { visualization_msgs::msg::MarkerArray marker_array; - - const auto now = clock_ptr_->now(); - - for (const auto & [id, traffic_light] : traffic_light_manager_->getTrafficLights()) { - traffic_light.draw(marker_array.markers, now, map_frame_); - } - - marker_pub_->publish(marker_array); + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array.markers.push_back(marker); + publisher_->publish(marker_array); } -auto TrafficLightMarkerPublisher::publish() -> void +auto TrafficLightMarkerPublisher::drawMarkers( + const std::unordered_map & traffic_lights_map) const -> void { - if (traffic_light_manager_->hasAnyLightChanged()) { - deleteAllMarkers(); + visualization_msgs::msg::MarkerArray marker_array; + for (const auto & [id, traffic_light] : traffic_lights_map) { + traffic_light.draw(marker_array.markers, clock_ptr_->now(), frame_); } - - drawMarkers(); + publisher_->publish(marker_array); } - } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp index e5a0689e24c..dfaee1e9098 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -14,25 +14,27 @@ #include #include +#include #include -#include namespace traffic_simulator { template <> auto TrafficLightPublisher::publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void + const TrafficLightsBase & traffic_lights) const -> void { + const auto states_as_proto_request = traffic_lights.generateUpdateTrafficLightsRequest(); autoware_auto_perception_msgs::msg::TrafficSignalArray message; - using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal; + message.header.frame_id = "camera_link"; // DIRTY HACK!!! - message.header.stamp = current_ros_time; - for (const auto & traffic_light : request.states()) { + message.header.stamp = clock_ptr_->now(); + + using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal; + using TrafficLightBulbType = TrafficLightType::_lights_type::value_type; + for (const auto & traffic_light : states_as_proto_request.states()) { TrafficLightType traffic_light_message; traffic_light_message.map_primitive_id = traffic_light.id(); for (const auto & bulb_status : traffic_light.traffic_light_status()) { - using TrafficLightBulbType = TrafficLightType::_lights_type::value_type; TrafficLightBulbType light_bulb_message; simulation_interface::toMsg(bulb_status, light_bulb_message); traffic_light_message.lights.push_back(light_bulb_message); @@ -44,27 +46,23 @@ auto TrafficLightPublisher auto TrafficLightPublisher::publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void + const TrafficLightsBase & traffic_lights) const -> void { - assert(hdmap_utils_ != nullptr); - + const auto states_as_proto_request = traffic_lights.generateUpdateTrafficLightsRequest(); autoware_perception_msgs::msg::TrafficSignalArray message; - message.stamp = current_ros_time; - for (const auto & traffic_light : request.states()) { - auto relation_ids = - hdmap_utils_->getTrafficLightRegulatoryElementIDsFromTrafficLight(traffic_light.id()); - for (const auto & relation_id : relation_ids) { + message.stamp = clock_ptr_->now(); + + using TrafficLightType = autoware_perception_msgs::msg::TrafficSignal; + using TrafficLightBulbType = + autoware_perception_msgs::msg::TrafficSignal::_elements_type::value_type; + for (const auto & traffic_light : states_as_proto_request.states()) { + for (const auto & relation_id : traffic_light.relation_ids()) { // skip if the traffic light has no bulbs if (not traffic_light.traffic_light_status().empty()) { - using TrafficLightType = autoware_perception_msgs::msg::TrafficSignal; TrafficLightType traffic_light_message; traffic_light_message.traffic_signal_id = relation_id; - for (const auto & bulb_status : traffic_light.traffic_light_status()) { - using TrafficLightBulbType = - autoware_perception_msgs::msg::TrafficSignal::_elements_type::value_type; TrafficLightBulbType light_bulb_message; simulation_interface::toMsg(bulb_status, light_bulb_message); traffic_light_message.elements.push_back(light_bulb_message); @@ -78,22 +76,8 @@ auto TrafficLightPublisher::p template <> auto TrafficLightPublisher::publish( - [[maybe_unused]] const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void + const TrafficLightsBase & traffic_lights) const -> void { - traffic_simulator_msgs::msg::TrafficLightArrayV1 message; - using TrafficLightType = traffic_simulator_msgs::msg::TrafficLightV1; - for (const auto & traffic_light : request.states()) { - TrafficLightType traffic_light_message; - traffic_light_message.lanelet_way_id = traffic_light.id(); - for (const auto & bulb_status : traffic_light.traffic_light_status()) { - using TrafficLightBulbType = traffic_simulator_msgs::msg::TrafficLightBulbV1; - TrafficLightBulbType light_bulb_message; - simulation_interface::toMsg(bulb_status, light_bulb_message); - traffic_light_message.traffic_light_bulbs.push_back(light_bulb_message); - } - message.traffic_lights.push_back(traffic_light_message); - } - traffic_light_state_array_publisher_->publish(message); + traffic_light_state_array_publisher_->publish(traffic_lights.generateTrafficSimulatorV1Msg()); } } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp new file mode 100644 index 00000000000..5f19b6a31f0 --- /dev/null +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights.cpp @@ -0,0 +1,43 @@ +// Copyright 2015 TIER IV, Inc. 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 + +namespace traffic_simulator +{ +auto TrafficLights::isAnyTrafficLightChanged() -> bool +{ + return conventional_traffic_lights_->isAnyTrafficLightChanged() or + v2i_traffic_lights_->isAnyTrafficLightChanged(); +} + +auto TrafficLights::startTrafficLightsUpdate( + const double conventional_traffic_light_update_rate, const double v2i_traffic_lights_update_rate) + -> void +{ + conventional_traffic_lights_->startUpdate(conventional_traffic_light_update_rate); + v2i_traffic_lights_->startUpdate(v2i_traffic_lights_update_rate); +} + +auto TrafficLights::getConventionalTrafficLights() const + -> std::shared_ptr +{ + return conventional_traffic_lights_; +} + +auto TrafficLights::getV2ITrafficLights() const -> std::shared_ptr +{ + return v2i_traffic_lights_; +} +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp new file mode 100644 index 00000000000..620a666d68c --- /dev/null +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp @@ -0,0 +1,160 @@ +// Copyright 2015 TIER IV, Inc. 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 + +namespace traffic_simulator +{ +auto TrafficLightsBase::startUpdate(const double update_rate) -> void +{ + rate_updater_.startTimer(update_rate); +} + +auto TrafficLightsBase::resetUpdate(const double update_rate) -> void +{ + rate_updater_.resetTimer(update_rate); +} + +// checks, setters, getters +auto TrafficLightsBase::isAnyTrafficLightChanged() const -> bool { return true; } + +auto TrafficLightsBase::isRequiredStopTrafficLightState(const lanelet::Id traffic_light_id) -> bool +{ + using Color = traffic_simulator::TrafficLight::Color; + using Status = traffic_simulator::TrafficLight::Status; + using Shape = traffic_simulator::TrafficLight::Shape; + const auto & traffic_light = getTrafficLight(traffic_light_id); + return ( + traffic_light.contains(Color::red, Status::solid_on, Shape::circle) or + traffic_light.contains(Color::yellow, Status::solid_on, Shape::circle)); +} + +auto TrafficLightsBase::compareTrafficLightsState( + const lanelet::Id lanelet_id, const std::string & state) -> bool +{ + if (const auto & considered_traffic_lights = getTrafficLights(lanelet_id); + state.empty() || state == "none") { + return std::all_of( + std::begin(considered_traffic_lights), std::end(considered_traffic_lights), + [](const auto & considered_traffic_light) { return considered_traffic_light.get().empty(); }); + } else { + return std::all_of( + std::begin(considered_traffic_lights), std::end(considered_traffic_lights), + [&state](const auto & considered_traffic_light) { + return considered_traffic_light.get().contains(state); + }); + } +} + +auto TrafficLightsBase::setTrafficLightsColor( + const lanelet::Id lanelet_id, const traffic_simulator::TrafficLight::Color & color) -> void +{ + for (const auto & traffic_light : getTrafficLights(lanelet_id)) { + traffic_light.get().emplace(color); + } +} + +auto TrafficLightsBase::setTrafficLightsState( + const lanelet::Id lanelet_id, const std::string & state) -> void +{ + for (const auto & traffic_light : getTrafficLights(lanelet_id)) { + traffic_light.get().clear(); + traffic_light.get().set(state); + } +} + +auto TrafficLightsBase::setTrafficLightsConfidence( + const lanelet::Id lanelet_id, const double confidence) -> void +{ + for (auto & traffic_light : getTrafficLights(lanelet_id)) { + traffic_light.get().confidence = confidence; + } +} + +auto TrafficLightsBase::getTrafficLightsComposedState(const lanelet::Id lanelet_id) -> std::string +{ + std::stringstream ss; + std::string separator = ""; + for (const auto & traffic_light : getTrafficLights(lanelet_id)) { + ss << separator << traffic_light; + separator = "; "; + } + return ss.str(); +} + +auto TrafficLightsBase::generateUpdateTrafficLightsRequest() const + -> simulation_api_schema::UpdateTrafficLightsRequest +{ + simulation_api_schema::UpdateTrafficLightsRequest update_traffic_lights_request; + for (auto && [lanelet_id, traffic_light] : traffic_lights_map_) { + auto traffic_signal = static_cast(traffic_light); + for (const auto & relation_id : traffic_light.regulatory_elements_ids) { + traffic_signal.add_relation_ids(relation_id); + } + *update_traffic_lights_request.add_states() = traffic_signal; + } + return update_traffic_lights_request; +} + +auto TrafficLightsBase::generateTrafficSimulatorV1Msg() const + -> traffic_simulator_msgs::msg::TrafficLightArrayV1 +{ + traffic_simulator_msgs::msg::TrafficLightArrayV1 traffic_lights_message; + for (const auto & [id, traffic_light] : traffic_lights_map_) { + traffic_lights_message.traffic_lights.push_back( + static_cast(traffic_light)); + } + return traffic_lights_message; +} + +// private +auto TrafficLightsBase::isTrafficLightAdded(const lanelet::Id traffic_light_id) const -> bool +{ + return traffic_lights_map_.find(traffic_light_id) != traffic_lights_map_.end(); +} + +auto TrafficLightsBase::addTrafficLight(const lanelet::Id traffic_light_id) -> void +{ + // emplace will not modify the map if the key already exists + traffic_lights_map_.emplace( + std::piecewise_construct, std::forward_as_tuple(traffic_light_id), + std::forward_as_tuple(traffic_light_id, *hdmap_utils_)); +} + +auto TrafficLightsBase::getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight & +{ + addTrafficLight(traffic_light_id); + return traffic_lights_map_.at(traffic_light_id); +} + +auto TrafficLightsBase::getTrafficLights(const lanelet::Id lanelet_id) + -> std::vector> +{ + // if passed id is regulatory element containing traffic_lights, add all of them + // if passed id is single traffic_light - add it, return all added traffic_lights + std::vector> traffic_lights; + if (hdmap_utils_->isTrafficLightRegulatoryElement(lanelet_id)) { + const auto & regulatory_element = hdmap_utils_->getTrafficLightRegulatoryElement(lanelet_id); + for (auto && traffic_light : regulatory_element->trafficLights()) { + traffic_lights.emplace_back(getTrafficLight(traffic_light.id())); + } + } else if (hdmap_utils_->isTrafficLight(lanelet_id)) { + traffic_lights.emplace_back(getTrafficLight(lanelet_id)); + } else { + throw common::scenario_simulator_exception::Error( + "Given lanelet ID ", lanelet_id, " is neither a traffic light ID not a traffic relation ID."); + } + return traffic_lights; +} +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 63f39a58897..a8d9b6175f3 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 TIER IV, Inc. 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. diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 32cabd15b44..36cf69ca25a 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 TIER IV, Inc. 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. diff --git a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt index 23df37b3bd0..3bc7c5c8d69 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/traffic_lights/CMakeLists.txt @@ -1,5 +1,2 @@ ament_add_gtest(test_traffic_light test_traffic_light.cpp) target_link_libraries(test_traffic_light traffic_simulator) - -ament_add_gtest(test_traffic_light_manager test_traffic_light_manager.cpp) -target_link_libraries(test_traffic_light_manager traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp deleted file mode 100644 index 3593b18f50b..00000000000 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright 2015 TIER IV, Inc. 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 - -#include -#include -#include - -class TrafficLightManagerTest : public testing::Test -{ -protected: - TrafficLightManagerTest() - : manager(std::make_shared( - ament_index_cpp::get_package_share_directory("traffic_simulator") + - "/map/standard_map/lanelet2_map.osm", - geographic_msgs::build() - .latitude(35.61836750154) - .longitude(139.78066608243) - .altitude(0.0))) - { - } - traffic_simulator::TrafficLightManager manager; -}; - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - -/** - * @note Test basic functionality. Test obtaining traffic light functionality - * with a non-existant laneletId. A traffic light should be created. - */ -TEST_F(TrafficLightManagerTest, getTrafficLight) -{ - manager.getTrafficLight(34836); - EXPECT_FALSE(manager.getTrafficLights().find(34836) == std::end(manager.getTrafficLights())); - manager.getTrafficLight(34802); - EXPECT_FALSE(manager.getTrafficLights().find(34802) == std::end(manager.getTrafficLights())); - EXPECT_EQ(manager.getTrafficLights().size(), static_cast(2)); -} - -/** - * @note Test basic functionality. Test adding a bulb to a specific traffic light with only a color specified. - */ -TEST_F(TrafficLightManagerTest, getTrafficLights_setColor) -{ - using Color = traffic_simulator::TrafficLight::Color; - using Status = traffic_simulator::TrafficLight::Status; - using Shape = traffic_simulator::TrafficLight::Shape; - for (const auto & [id, traffic_light] : manager.getTrafficLights()) { - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::green); - EXPECT_TRUE( - manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::circle)); - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::yellow); - EXPECT_TRUE( - manager.getTrafficLight(id).contains(Color::yellow, Status::solid_on, Shape::circle)); - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::red); - EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::red, Status::solid_on, Shape::circle)); - } -} - -/** - * @note Test basic functionality. Test adding a bulb to a specific traffic light with all parameters specified. - */ -TEST_F(TrafficLightManagerTest, getTrafficLights_setArrow) -{ - using Color = traffic_simulator::TrafficLight::Color; - using Status = traffic_simulator::TrafficLight::Status; - using Shape = traffic_simulator::TrafficLight::Shape; - for (const auto & [id, traffic_light] : manager.getTrafficLights()) { - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::left); - EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::left)); - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::right); - EXPECT_TRUE( - manager.getTrafficLight(id).contains(Color::yellow, Status::solid_on, Shape::right)); - manager.getTrafficLight(id).clear(); - manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::up); - EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::up)); - } -}