15 #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_HPP_
16 #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_HPP_
18 #include <color_names/color_names.hpp>
20 #include <geometry_msgs/msg/point.hpp>
32 #include <type_traits>
33 #include <unordered_map>
51 static_assert(
static_cast<std::uint8_t
>(
green ) == 0b0000'0000);
52 static_assert(
static_cast<std::uint8_t
>(
yellow) == 0b0000'0001);
53 static_assert(
static_cast<std::uint8_t
>(
red ) == 0b0000'0010);
54 static_assert(
static_cast<std::uint8_t
>(
white ) == 0b0000'0011);
61 static inline const std::unordered_map<std::string, Value>
table{
62 std::make_pair(
"amber",
yellow),
63 std::make_pair(
"green",
green),
64 std::make_pair(
"red",
red),
65 std::make_pair(
"white",
white),
66 std::make_pair(
"yellow",
yellow),
69 std::make_pair(
"Green",
green),
70 std::make_pair(
"Red",
red),
71 std::make_pair(
"Yellow",
yellow),
76 constexpr
auto is(
const Color given)
const {
return value == given; }
78 constexpr
operator Value() const noexcept {
return value; }
82 friend auto operator<<(std::ostream & os,
const Color & color) -> std::ostream &;
95 static_assert(
static_cast<std::uint8_t
>(
solid_on ) == 0b0000'0000);
96 static_assert(
static_cast<std::uint8_t
>(
solid_off) == 0b0000'0001);
97 static_assert(
static_cast<std::uint8_t
>(
flashing ) == 0b0000'0010);
98 static_assert(
static_cast<std::uint8_t
>(
unknown ) == 0b0000'0011);
105 static inline const std::unordered_map<std::string, Value>
table{
106 std::make_pair(
"solidOn",
solid_on),
108 std::make_pair(
"flashing",
flashing),
109 std::make_pair(
"unknown",
unknown),
126 friend auto operator<<(std::ostream & os,
const Status & status) -> std::ostream &;
139 static_assert(
static_cast<std::uint8_t
>(
Category::cross ) == 0b0000'0001);
140 static_assert(
static_cast<std::uint8_t
>(
Category::arrow ) == 0b0000'0010);
159 static_assert(
static_cast<std::uint16_t
>(
circle ) == 0b0000'0000'0000'0000);
160 static_assert(
static_cast<std::uint16_t
>(
cross ) == 0b0000'0000'0000'0001);
161 static_assert(
static_cast<std::uint16_t
>(
left ) == 0b0000'1000'0000'0010);
162 static_assert(
static_cast<std::uint16_t
>(
down ) == 0b0000'0100'0000'0010);
163 static_assert(
static_cast<std::uint16_t
>(
up ) == 0b0000'0010'0000'0010);
164 static_assert(
static_cast<std::uint16_t
>(
right ) == 0b0000'0001'0000'0010);
165 static_assert(
static_cast<std::uint16_t
>(
lower_left ) == 0b0000'1100'0000'0010);
166 static_assert(
static_cast<std::uint16_t
>(
upper_left ) == 0b0000'1010'0000'0010);
167 static_assert(
static_cast<std::uint16_t
>(
lower_right) == 0b0000'0101'0000'0010);
168 static_assert(
static_cast<std::uint16_t
>(
upper_right) == 0b0000'0011'0000'0010);
175 static inline const std::unordered_map<std::string, Shape::Value>
table{
195 return static_cast<Category>(
static_cast<std::uint16_t
>(
value) & 0b1111'1111);
206 friend auto operator<<(std::ostream & os,
const Shape & shape) -> std::ostream &;
211 using Value = std::tuple<Color, Status, Shape>;
219 constexpr
Bulb(
const Color color = {},
const Status status = {},
const Shape shape = {})
220 :
Bulb(std::forward_as_tuple(color, status, shape))
228 constexpr
auto is(
const Color color)
const {
return std::get<Color>(
value).is(color); }
230 constexpr
auto is(
const Status status)
const {
return std::get<Status>(
value).is(status); }
232 constexpr
auto is(
const Shape shape)
const {
return std::get<Shape>(
value).is(shape); }
236 return std::get<Shape>(
value).is(category);
242 (
static_cast<Hash>(std::get<Status>(
value).value) << 16) |
243 static_cast<Hash>(std::get<Shape>(
value).value);
248 return lhs.hash() < rhs.hash();
251 friend auto operator<<(std::ostream & os,
const Bulb & bulb) -> std::ostream &;
253 explicit operator simulation_api_schema::TrafficLight()
const
255 auto color = [
this]() {
256 switch (std::get<Color>(
value).value) {
258 return simulation_api_schema::TrafficLight_Color_GREEN;
260 return simulation_api_schema::TrafficLight_Color_AMBER;
262 return simulation_api_schema::TrafficLight_Color_RED;
264 return simulation_api_schema::TrafficLight_Color_WHITE;
270 auto status = [
this]() {
271 switch (std::get<Status>(
value).value) {
273 return simulation_api_schema::TrafficLight_Status_SOLID_ON;
275 return simulation_api_schema::TrafficLight_Status_SOLID_OFF;
277 return simulation_api_schema::TrafficLight_Status_FLASHING;
279 return simulation_api_schema::TrafficLight_Status_UNKNOWN_STATUS;
285 auto shape = [
this]() {
286 switch (std::get<Shape>(
value).value) {
288 return simulation_api_schema::TrafficLight_Shape_CIRCLE;
290 return simulation_api_schema::TrafficLight_Shape_CROSS;
292 return simulation_api_schema::TrafficLight_Shape_LEFT_ARROW;
294 return simulation_api_schema::TrafficLight_Shape_DOWN_ARROW;
296 return simulation_api_schema::TrafficLight_Shape_UP_ARROW;
298 return simulation_api_schema::TrafficLight_Shape_RIGHT_ARROW;
300 return simulation_api_schema::TrafficLight_Shape_DOWN_LEFT_ARROW;
302 return simulation_api_schema::TrafficLight_Shape_DOWN_RIGHT_ARROW;
304 return simulation_api_schema::TrafficLight_Shape_UP_LEFT_ARROW;
306 return simulation_api_schema::TrafficLight_Shape_UP_RIGHT_ARROW;
312 simulation_api_schema::TrafficLight traffic_light_bulb_proto;
313 traffic_light_bulb_proto.set_status(status());
314 traffic_light_bulb_proto.set_shape(shape());
315 traffic_light_bulb_proto.set_color(color());
317 traffic_light_bulb_proto.set_confidence(1.0);
319 return traffic_light_bulb_proto;
329 const std::map<Bulb::Hash, std::optional<geometry_msgs::msg::Point>>
positions;
344 template <
typename Markers,
typename Now>
347 auto optional_position = [
this](
auto && bulb) {
349 return positions.at(bulb.hash() & 0b1111'0000'1111'1111);
350 }
catch (
const std::out_of_range &) {
351 return std::optional<geometry_msgs::msg::Point>(std::nullopt);
355 for (
const auto & bulb :
bulbs) {
357 visualization_msgs::msg::Marker marker;
358 marker.header.stamp = now;
359 marker.header.frame_id = frame_id;
360 marker.action = marker.ADD;
363 marker.type = marker.SPHERE;
364 marker.pose.position = optional_position(bulb).value();
365 marker.pose.orientation = geometry_msgs::msg::Quaternion();
366 marker.scale.x = 0.3;
367 marker.scale.y = 0.3;
368 marker.scale.z = 0.3;
370 color_names::makeColorMsg(boost::lexical_cast<std::string>(std::get<Color>(bulb.value)));
371 markers.push_back(marker);
376 template <
typename... Ts>
379 bulbs.emplace(std::forward<decltype(
xs)>(
xs)...);
388 explicit operator simulation_api_schema::TrafficSignal()
const
390 simulation_api_schema::TrafficSignal traffic_signal_proto;
392 traffic_signal_proto.set_id(
way_id);
393 for (
const auto & bulb :
bulbs) {
394 auto traffic_light_bulb_proto =
static_cast<simulation_api_schema::TrafficLight
>(bulb);
395 traffic_light_bulb_proto.set_confidence(
confidence);
396 *traffic_signal_proto.add_traffic_light_status() = traffic_light_bulb_proto;
398 return traffic_signal_proto;
Definition: hdmap_utils.hpp:62
Definition: junit5.hpp:25
std::string string
Definition: junit5.hpp:26
There is a syntactic error in the description of the scenario. Or you are using a feature that is not...
Definition: exception.hpp:53
Definition: traffic_light.hpp:210
constexpr auto is(const Shape shape) const
Definition: traffic_light.hpp:232
std::tuple< Color, Status, Shape > Value
Definition: traffic_light.hpp:211
constexpr auto hash() const -> Hash
Definition: traffic_light.hpp:239
constexpr Bulb(const Value value)
Definition: traffic_light.hpp:217
constexpr Bulb(const Color color={}, const Status status={}, const Shape shape={})
Definition: traffic_light.hpp:219
Bulb(const std::string &name)
Definition: traffic_light.hpp:224
constexpr friend auto operator<(const Bulb &lhs, const Bulb &rhs) -> bool
Definition: traffic_light.hpp:246
constexpr auto is(const Color color) const
Definition: traffic_light.hpp:228
friend auto operator<<(std::ostream &os, const Bulb &bulb) -> std::ostream &
Definition: traffic_light.cpp:162
auto make(const std::string &s) -> Value
Definition: traffic_light.cpp:136
constexpr auto is(const Shape::Category category) const
Definition: traffic_light.hpp:234
const Value value
Definition: traffic_light.hpp:213
std::uint32_t Hash
Definition: traffic_light.hpp:215
constexpr auto is(const Status status) const
Definition: traffic_light.hpp:230
Definition: traffic_light.hpp:42
Value
Definition: traffic_light.hpp:43
@ white
Definition: traffic_light.hpp:47
@ green
Definition: traffic_light.hpp:44
@ red
Definition: traffic_light.hpp:46
@ yellow
Definition: traffic_light.hpp:45
friend auto operator<<(std::ostream &os, const Color &color) -> std::ostream &
Definition: traffic_light.cpp:43
constexpr Color(const Value value=green)
Definition: traffic_light.hpp:57
Color(const std::string &name)
Definition: traffic_light.hpp:59
static const std::unordered_map< std::string, Value > table
Definition: traffic_light.hpp:61
enum traffic_simulator::TrafficLight::Color::Value value
friend auto operator>>(std::istream &is, Color &color) -> std::istream &
Definition: traffic_light.cpp:35
static auto make(const std::string &name) -> Color
Definition: traffic_light.cpp:26
constexpr auto is(const Color given) const
Definition: traffic_light.hpp:76
Definition: traffic_light.hpp:130
Value
Definition: traffic_light.hpp:143
@ up
Definition: traffic_light.hpp:149
@ upper_right
Definition: traffic_light.hpp:154
@ left
Definition: traffic_light.hpp:147
@ cross
Definition: traffic_light.hpp:146
@ lower_right
Definition: traffic_light.hpp:153
@ upper_left
Definition: traffic_light.hpp:152
@ lower_left
Definition: traffic_light.hpp:151
@ down
Definition: traffic_light.hpp:148
@ circle
Definition: traffic_light.hpp:145
@ right
Definition: traffic_light.hpp:150
constexpr auto category() const
Definition: traffic_light.hpp:193
enum traffic_simulator::TrafficLight::Shape::Value value
static auto make(const std::string &name) -> Shape
Definition: traffic_light.cpp:91
constexpr auto is(const Category given) const
Definition: traffic_light.hpp:200
friend auto operator>>(std::istream &is, Shape &shape) -> std::istream &
Definition: traffic_light.cpp:100
Category
Definition: traffic_light.hpp:131
friend auto operator<<(std::ostream &os, const Shape &shape) -> std::ostream &
Definition: traffic_light.cpp:108
constexpr auto is(const Value given) const
Definition: traffic_light.hpp:198
constexpr Shape(const Value value=circle)
Definition: traffic_light.hpp:171
static const std::unordered_map< std::string, Shape::Value > table
Definition: traffic_light.hpp:175
Shape(const std::string &name)
Definition: traffic_light.hpp:173
Definition: traffic_light.hpp:86
constexpr auto is(const Value given) const
Definition: traffic_light.hpp:118
constexpr Status(const Value value=solid_on)
Definition: traffic_light.hpp:101
static const std::unordered_map< std::string, Value > table
Definition: traffic_light.hpp:105
Status(const std::string &name)
Definition: traffic_light.hpp:103
Value
Definition: traffic_light.hpp:87
@ solid_off
Definition: traffic_light.hpp:89
@ unknown
Definition: traffic_light.hpp:91
@ solid_on
Definition: traffic_light.hpp:88
@ flashing
Definition: traffic_light.hpp:90
friend auto operator<<(std::ostream &os, const Status &status) -> std::ostream &
Definition: traffic_light.cpp:76
enum traffic_simulator::TrafficLight::Status::Value value
static auto make(const std::string &name) -> Status
Definition: traffic_light.cpp:59
friend auto operator>>(std::istream &is, Status &status) -> std::istream &
Definition: traffic_light.cpp:68
Definition: traffic_light.hpp:40
auto clear()
Definition: traffic_light.hpp:333
double confidence
Definition: traffic_light.hpp:325
const lanelet::Id way_id
Definition: traffic_light.hpp:323
TrafficLight(const lanelet::Id, hdmap_utils::HdMapUtils &)
Definition: traffic_light.cpp:169
const std::map< Bulb::Hash, std::optional< geometry_msgs::msg::Point > > positions
Definition: traffic_light.hpp:329
auto empty() const
Definition: traffic_light.hpp:382
auto contains(const Color &color, const Status &status, const Shape &shape) const
Definition: traffic_light.hpp:337
std::set< Bulb > bulbs
Definition: traffic_light.hpp:327
auto set(const std::string &states) -> void
Definition: traffic_light.cpp:201
auto contains(const std::string &name) const
Definition: traffic_light.hpp:342
auto draw(Markers &markers, const Now &now, const std::string &frame_id) const
Definition: traffic_light.hpp:345
friend auto operator<<(std::ostream &os, const TrafficLight &traffic_light) -> std::ostream &
Definition: traffic_light.cpp:219
auto contains(const Bulb &bulb) const
Definition: traffic_light.hpp:335
auto emplace(Ts &&... xs)
Definition: traffic_light.hpp:377