15 #ifndef SIMULATION_INTERFACE__CONVERSIONS_HPP_
16 #define SIMULATION_INTERFACE__CONVERSIONS_HPP_
18 #include <builtin_interfaces.pb.h>
19 #include <geometry_msgs.pb.h>
20 #include <rosgraph_msgs.pb.h>
21 #include <simulation_api_schema.pb.h>
22 #include <std_msgs.pb.h>
23 #include <traffic_simulator_msgs.pb.h>
25 #include <autoware_control_msgs/msg/control.hpp>
26 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
27 #include <builtin_interfaces/msg/duration.hpp>
28 #include <builtin_interfaces/msg/time.hpp>
29 #include <geometry_msgs/msg/accel.hpp>
30 #include <geometry_msgs/msg/point.hpp>
31 #include <geometry_msgs/msg/pose.hpp>
32 #include <geometry_msgs/msg/quaternion.hpp>
33 #include <geometry_msgs/msg/twist.hpp>
34 #include <geometry_msgs/msg/vector3.hpp>
36 #include <rosgraph_msgs/msg/clock.hpp>
38 #include <std_msgs/msg/header.hpp>
40 #include <traffic_simulator_msgs/msg/action_status.hpp>
41 #include <traffic_simulator_msgs/msg/axle.hpp>
42 #include <traffic_simulator_msgs/msg/axles.hpp>
43 #include <traffic_simulator_msgs/msg/bounding_box.hpp>
44 #include <traffic_simulator_msgs/msg/entity_status.hpp>
45 #include <traffic_simulator_msgs/msg/entity_type.hpp>
46 #include <traffic_simulator_msgs/msg/lanelet_pose.hpp>
47 #include <traffic_simulator_msgs/msg/misc_object_parameters.hpp>
48 #include <traffic_simulator_msgs/msg/pedestrian_parameters.hpp>
49 #include <traffic_simulator_msgs/msg/performance.hpp>
50 #include <traffic_simulator_msgs/msg/polyline_trajectory.hpp>
51 #include <traffic_simulator_msgs/msg/vehicle_parameters.hpp>
53 #include <zmqpp/zmqpp.hpp>
57 template <
typename Proto>
58 zmqpp::message
toZMQ(
const Proto & proto)
62 proto.SerializeToString(&serialized_str);
63 msg << serialized_str;
67 template <
typename Proto>
72 proto.ParseFromString(serialized_str);
81 void toProto(
const geometry_msgs::msg::Quaternion & q, geometry_msgs::Quaternion & proto);
82 void toMsg(
const geometry_msgs::Quaternion & proto, geometry_msgs::msg::Quaternion & q);
87 void toProto(
const geometry_msgs::msg::Twist & t, geometry_msgs::Twist & proto);
88 void toMsg(
const geometry_msgs::Twist & proto, geometry_msgs::msg::Twist & t);
89 void toProto(
const geometry_msgs::msg::Accel & a, geometry_msgs::Accel & proto);
90 void toMsg(
const geometry_msgs::Accel & proto, geometry_msgs::msg::Accel & a);
98 const traffic_simulator_msgs::msg::Performance & performance,
99 traffic_simulator_msgs::Performance & proto);
101 const traffic_simulator_msgs::Performance & proto,
102 traffic_simulator_msgs::msg::Performance & performance);
103 void toProto(
const traffic_simulator_msgs::msg::Axle & axle, traffic_simulator_msgs::Axle & proto);
104 void toMsg(
const traffic_simulator_msgs::Axle & proto, traffic_simulator_msgs::msg::Axle & axle);
106 const traffic_simulator_msgs::msg::Axles & axles, traffic_simulator_msgs::Axles & proto);
107 void toMsg(
const traffic_simulator_msgs::Axles & proto, traffic_simulator_msgs::msg::Axles & axles);
109 const traffic_simulator_msgs::msg::VehicleParameters & p,
110 traffic_simulator_msgs::VehicleParameters & proto);
112 const traffic_simulator_msgs::VehicleParameters & proto,
113 traffic_simulator_msgs::msg::VehicleParameters & p);
115 const traffic_simulator_msgs::msg::PedestrianParameters & p,
116 traffic_simulator_msgs::PedestrianParameters & proto);
118 const traffic_simulator_msgs::PedestrianParameters & proto,
119 traffic_simulator_msgs::msg::PedestrianParameters & p);
121 const traffic_simulator_msgs::msg::MiscObjectParameters & p,
122 traffic_simulator_msgs::MiscObjectParameters & proto);
124 const traffic_simulator_msgs::MiscObjectParameters & proto,
125 traffic_simulator_msgs::msg::MiscObjectParameters & p);
127 const traffic_simulator_msgs::msg::ActionStatus & s,
128 traffic_simulator_msgs::ActionStatus & proto);
130 const traffic_simulator_msgs::ActionStatus & proto,
131 traffic_simulator_msgs::msg::ActionStatus & s);
161 const builtin_interfaces::msg::Duration & duration, builtin_interfaces::Duration & proto);
163 const builtin_interfaces::Duration & proto, builtin_interfaces::msg::Duration & duration);
164 void toProto(
const builtin_interfaces::msg::Time & time, builtin_interfaces::Time & proto);
165 void toMsg(
const builtin_interfaces::Time & proto, builtin_interfaces::msg::Time & time);
166 void toProto(
const rosgraph_msgs::msg::Clock & time, rosgraph_msgs::Clock & proto);
167 void toMsg(
const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time);
168 void toProto(
const std_msgs::msg::Header & header, std_msgs::Header & proto);
169 void toMsg(
const std_msgs::Header & proto, std_msgs::msg::Header & header);
172 #define DEFINE_CONVERSION(PACKAGE, TYPENAME) \
173 auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \
174 auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void
181 #undef DEFINE_CONVERSION
184 const std::tuple<autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> &,
185 traffic_simulator_msgs::VehicleCommand &) -> void;
187 template <
typename TrafficLightBulbMessageType>
190 TrafficLightBulbMessageType & traffic_light_bulb_state) ->
void
192 using namespace simulation_api_schema;
194 auto convert_color = [](
auto color) constexpr
197 case TrafficLight_Color_RED:
198 return TrafficLightBulbMessageType::RED;
199 case TrafficLight_Color_AMBER:
200 return TrafficLightBulbMessageType::AMBER;
201 case TrafficLight_Color_GREEN:
202 return TrafficLightBulbMessageType::GREEN;
203 case TrafficLight_Color_WHITE:
204 return TrafficLightBulbMessageType::WHITE;
206 return TrafficLightBulbMessageType::UNKNOWN;
210 auto convert_shape = [](
auto shape) constexpr
213 case TrafficLight_Shape_CIRCLE:
214 return TrafficLightBulbMessageType::CIRCLE;
215 case TrafficLight_Shape_LEFT_ARROW:
216 return TrafficLightBulbMessageType::LEFT_ARROW;
217 case TrafficLight_Shape_RIGHT_ARROW:
218 return TrafficLightBulbMessageType::RIGHT_ARROW;
219 case TrafficLight_Shape_UP_ARROW:
220 return TrafficLightBulbMessageType::UP_ARROW;
226 case TrafficLight_Shape_DOWN_ARROW:
227 return TrafficLightBulbMessageType::DOWN_ARROW;
228 case TrafficLight_Shape_DOWN_LEFT_ARROW:
229 return TrafficLightBulbMessageType::DOWN_LEFT_ARROW;
230 case TrafficLight_Shape_DOWN_RIGHT_ARROW:
231 return TrafficLightBulbMessageType::DOWN_RIGHT_ARROW;
232 case TrafficLight_Shape_CROSS:
233 return TrafficLightBulbMessageType::CROSS;
235 return TrafficLightBulbMessageType::UNKNOWN;
239 auto convert_status = [](
auto status) constexpr
242 case TrafficLight_Status_SOLID_OFF:
243 return TrafficLightBulbMessageType::SOLID_OFF;
244 case TrafficLight_Status_SOLID_ON:
245 return TrafficLightBulbMessageType::SOLID_ON;
246 case TrafficLight_Status_FLASHING:
247 return TrafficLightBulbMessageType::FLASHING;
249 return TrafficLightBulbMessageType::UNKNOWN;
253 traffic_light_bulb_state.status = convert_status(proto.status());
254 traffic_light_bulb_state.shape = convert_shape(proto.shape());
255 traffic_light_bulb_state.color = convert_color(proto.color());
256 traffic_light_bulb_state.confidence = proto.confidence();
261 -> traffic_simulator_msgs::Vertex;
263 auto toROS2Message(
const traffic_simulator_msgs::Vertex &) -> traffic_simulator_msgs::msg::Vertex;
266 -> traffic_simulator_msgs::Polyline;
269 -> traffic_simulator_msgs::msg::Polyline;
272 -> traffic_simulator_msgs::PolylineTrajectory;
274 auto toROS2Message(
const traffic_simulator_msgs::PolylineTrajectory &)
275 -> traffic_simulator_msgs::msg::PolylineTrajectory;
Definition: constants.hpp:21
auto toProtobufMessage(const traffic_simulator_msgs::msg::Vertex &) -> traffic_simulator_msgs::Vertex
Definition: conversions.cpp:601
void toProto(const geometry_msgs::msg::Point &p, geometry_msgs::Point &proto)
Definition: conversions.cpp:22
DEFINE_CONVERSION(autoware_control_msgs, Lateral)
void toMsg(const geometry_msgs::Point &proto, geometry_msgs::msg::Point &p)
Definition: conversions.cpp:29
auto toROS2Message(const traffic_simulator_msgs::Vertex &) -> traffic_simulator_msgs::msg::Vertex
Definition: conversions.cpp:610
geometry_msgs::msg::Pose Pose
Definition: lanelet_wrapper.hpp:66
traffic_simulator_msgs::msg::BoundingBox BoundingBox
Definition: lanelet_wrapper.hpp:62
geometry_msgs::msg::Point Point
Definition: lanelet_wrapper.hpp:65
geometry_msgs::msg::Vector3 Vector3
Definition: lanelet_wrapper.hpp:69
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:23
std::string string
Definition: junit5.hpp:26
Definition: conversions.hpp:56
Proto toProto(const zmqpp::message &msg)
Definition: conversions.hpp:68
zmqpp::message toZMQ(const Proto &proto)
Definition: conversions.hpp:58
traffic_simulator_msgs::EntitySubtype EntitySubtype
Definition: helper_functions.hpp:30
traffic_simulator_msgs::EntityStatus EntityStatus
Definition: helper_functions.hpp:32
traffic_simulator_msgs::EntityType EntityType
Definition: helper_functions.hpp:31
traffic_simulator::TrafficLight TrafficLight
Definition: test_traffic_light.cpp:24