scenario_simulator_v2 C++ API
conversions.hpp
Go to the documentation of this file.
1 // Copyright 2015 TIER IV, Inc. All rights reserved.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef SIMULATION_INTERFACE__CONVERSIONS_HPP_
16 #define SIMULATION_INTERFACE__CONVERSIONS_HPP_
17 
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>
24 
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>
35 #include <iostream>
36 #include <rosgraph_msgs/msg/clock.hpp>
38 #include <std_msgs/msg/header.hpp>
39 #include <string>
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>
52 #include <vector>
53 #include <zmqpp/zmqpp.hpp>
54 
55 namespace zeromq
56 {
57 template <typename Proto>
58 zmqpp::message toZMQ(const Proto & proto)
59 {
60  zmqpp::message msg;
61  std::string serialized_str = "";
62  proto.SerializeToString(&serialized_str);
63  msg << serialized_str;
64  return msg;
65 }
66 
67 template <typename Proto>
68 Proto toProto(const zmqpp::message & msg)
69 {
70  std::string serialized_str = msg.get(0);
71  Proto proto;
72  proto.ParseFromString(serialized_str);
73  return proto;
74 }
75 } // namespace zeromq
76 
77 namespace simulation_interface
78 {
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);
84 void toMsg(const geometry_msgs::Pose & proto, geometry_msgs::msg::Pose & p);
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);
91 void toProto(
94 void toMsg(
97 void toProto(
98  const traffic_simulator_msgs::msg::Performance & performance,
99  traffic_simulator_msgs::Performance & proto);
100 void toMsg(
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);
105 void toProto(
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);
108 void toProto(
109  const traffic_simulator_msgs::msg::VehicleParameters & p,
110  traffic_simulator_msgs::VehicleParameters & proto);
111 void toMsg(
112  const traffic_simulator_msgs::VehicleParameters & proto,
113  traffic_simulator_msgs::msg::VehicleParameters & p);
114 void toProto(
115  const traffic_simulator_msgs::msg::PedestrianParameters & p,
116  traffic_simulator_msgs::PedestrianParameters & proto);
117 void toMsg(
118  const traffic_simulator_msgs::PedestrianParameters & proto,
119  traffic_simulator_msgs::msg::PedestrianParameters & p);
120 void toProto(
121  const traffic_simulator_msgs::msg::MiscObjectParameters & p,
122  traffic_simulator_msgs::MiscObjectParameters & proto);
123 void toMsg(
124  const traffic_simulator_msgs::MiscObjectParameters & proto,
125  traffic_simulator_msgs::msg::MiscObjectParameters & p);
126 void toProto(
127  const traffic_simulator_msgs::msg::ActionStatus & s,
128  traffic_simulator_msgs::ActionStatus & proto);
129 void toMsg(
130  const traffic_simulator_msgs::ActionStatus & proto,
131  traffic_simulator_msgs::msg::ActionStatus & s);
132 void toProto(
135 void toMsg(
138 void toProto(
140 void toMsg(
142 void toProto(
145 void toMsg(
148 void toProto(
151 void toProto(
154 void toMsg(
157 void toMsg(
160 void toProto(
161  const builtin_interfaces::msg::Duration & duration, builtin_interfaces::Duration & proto);
162 void toMsg(
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);
170 
171 // clang-format off
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
175 
176 DEFINE_CONVERSION(autoware_control_msgs, Lateral);
177 DEFINE_CONVERSION(autoware_control_msgs, Longitudinal);
178 DEFINE_CONVERSION(autoware_control_msgs, Control);
179 DEFINE_CONVERSION(autoware_vehicle_msgs, GearCommand);
180 
181 #undef DEFINE_CONVERSION
182 
183 auto toProto(
184  const std::tuple<autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> &,
185  traffic_simulator_msgs::VehicleCommand &) -> void;
186 
187 template <typename TrafficLightBulbMessageType>
188 auto toMsg(
190  TrafficLightBulbMessageType & traffic_light_bulb_state) -> void
191 {
192  using namespace simulation_api_schema;
193 
194  auto convert_color = [](auto color) constexpr
195  {
196  switch (color) {
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;
205  default:
206  return TrafficLightBulbMessageType::UNKNOWN;
207  }
208  };
209 
210  auto convert_shape = [](auto shape) constexpr
211  {
212  switch (shape) {
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;
222  // case TrafficLight_Shape_UP_LEFT_ARROW:
223  // return TrafficLightBulbMessageType::UP_LEFT_ARROW;
224  // case TrafficLight_Shape_UP_RIGHT_ARROW:
225  // return TrafficLightBulbMessageType::UP_RIGHT_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;
234  default:
235  return TrafficLightBulbMessageType::UNKNOWN;
236  }
237  };
238 
239  auto convert_status = [](auto status) constexpr
240  {
241  switch (status) {
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;
248  default:
249  return TrafficLightBulbMessageType::UNKNOWN;
250  }
251  };
252 
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();
257 }
258 // clang-format off
259 
260 auto toProtobufMessage(const traffic_simulator_msgs::msg::Vertex &)
261  -> traffic_simulator_msgs::Vertex;
262 
263 auto toROS2Message(const traffic_simulator_msgs::Vertex &) -> traffic_simulator_msgs::msg::Vertex;
264 
265 auto toProtobufMessage(const traffic_simulator_msgs::msg::Polyline &)
266  -> traffic_simulator_msgs::Polyline;
267 
268 auto toROS2Message(const traffic_simulator_msgs::Polyline &)
269  -> traffic_simulator_msgs::msg::Polyline;
270 
271 auto toProtobufMessage(const traffic_simulator_msgs::msg::PolylineTrajectory &)
272  -> traffic_simulator_msgs::PolylineTrajectory;
273 
274 auto toROS2Message(const traffic_simulator_msgs::PolylineTrajectory &)
275  -> traffic_simulator_msgs::msg::PolylineTrajectory;
276 } // namespace simulation_interface
277 
278 #endif // SIMULATION_INTERFACE__CONVERSIONS_HPP_
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