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 {
79 void toProto(const geometry_msgs::msg::Point & p, geometry_msgs::Point & proto);
80 void toMsg(const geometry_msgs::Point & proto, geometry_msgs::msg::Point & p);
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);
83 void toProto(const geometry_msgs::msg::Pose & p, geometry_msgs::Pose & proto);
84 void toMsg(const geometry_msgs::Pose & proto, geometry_msgs::msg::Pose & p);
85 void toProto(const geometry_msgs::msg::Vector3 & v, geometry_msgs::Vector3 & proto);
86 void toMsg(const geometry_msgs::Vector3 & proto, geometry_msgs::msg::Vector3 & v);
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(
92  const traffic_simulator_msgs::msg::BoundingBox & box,
93  traffic_simulator_msgs::BoundingBox & proto);
94 void toMsg(
95  const traffic_simulator_msgs::BoundingBox & proto,
96  traffic_simulator_msgs::msg::BoundingBox & box);
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 #define DEFINE_CONVERSION(PACKAGE, TYPENAME) \
172  auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \
173  auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void
174 
175 DEFINE_CONVERSION(autoware_control_msgs, Lateral);
176 DEFINE_CONVERSION(autoware_control_msgs, Longitudinal);
177 DEFINE_CONVERSION(autoware_control_msgs, Control);
178 DEFINE_CONVERSION(autoware_vehicle_msgs, GearCommand);
179 
180 #undef DEFINE_CONVERSION
181 
182 auto toProto(
183  const std::tuple<autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> &,
184  traffic_simulator_msgs::VehicleCommand &) -> void;
185 
186 template <typename TrafficLightBulbMessageType>
187 auto toMsg(
189  TrafficLightBulbMessageType & traffic_light_bulb_state) -> void
190 {
191  using namespace simulation_api_schema;
192 
193  auto convert_color = [](auto color) constexpr
194  {
195  switch (color) {
196  case TrafficLight_Color_RED:
197  return TrafficLightBulbMessageType::RED;
198  case TrafficLight_Color_AMBER:
199  return TrafficLightBulbMessageType::AMBER;
200  case TrafficLight_Color_GREEN:
201  return TrafficLightBulbMessageType::GREEN;
202  case TrafficLight_Color_WHITE:
203  return TrafficLightBulbMessageType::WHITE;
204  default:
205  return TrafficLightBulbMessageType::UNKNOWN;
206  }
207  };
208 
209  auto convert_shape = [](auto shape) constexpr
210  {
211  switch (shape) {
212  case TrafficLight_Shape_CIRCLE:
213  return TrafficLightBulbMessageType::CIRCLE;
214  case TrafficLight_Shape_LEFT_ARROW:
215  return TrafficLightBulbMessageType::LEFT_ARROW;
216  case TrafficLight_Shape_RIGHT_ARROW:
217  return TrafficLightBulbMessageType::RIGHT_ARROW;
218  case TrafficLight_Shape_UP_ARROW:
219  return TrafficLightBulbMessageType::UP_ARROW;
221  // case TrafficLight_Shape_UP_LEFT_ARROW:
222  // return TrafficLightBulbMessageType::UP_LEFT_ARROW;
223  // case TrafficLight_Shape_UP_RIGHT_ARROW:
224  // return TrafficLightBulbMessageType::UP_RIGHT_ARROW;
225  case TrafficLight_Shape_DOWN_ARROW:
226  return TrafficLightBulbMessageType::DOWN_ARROW;
227  case TrafficLight_Shape_DOWN_LEFT_ARROW:
228  return TrafficLightBulbMessageType::DOWN_LEFT_ARROW;
229  case TrafficLight_Shape_DOWN_RIGHT_ARROW:
230  return TrafficLightBulbMessageType::DOWN_RIGHT_ARROW;
231  case TrafficLight_Shape_CROSS:
232  return TrafficLightBulbMessageType::CROSS;
233  default:
234  return TrafficLightBulbMessageType::UNKNOWN;
235  }
236  };
237 
238  auto convert_status = [](auto status) constexpr
239  {
240  switch (status) {
241  case TrafficLight_Status_SOLID_OFF:
242  return TrafficLightBulbMessageType::SOLID_OFF;
243  case TrafficLight_Status_SOLID_ON:
244  return TrafficLightBulbMessageType::SOLID_ON;
245  case TrafficLight_Status_FLASHING:
246  return TrafficLightBulbMessageType::FLASHING;
247  default:
248  return TrafficLightBulbMessageType::UNKNOWN;
249  }
250  };
251 
252  traffic_light_bulb_state.status = convert_status(proto.status());
253  traffic_light_bulb_state.shape = convert_shape(proto.shape());
254  traffic_light_bulb_state.color = convert_color(proto.color());
255  traffic_light_bulb_state.confidence = proto.confidence();
256 }
257 
258 auto toProtobufMessage(const traffic_simulator_msgs::msg::Vertex &)
259  -> traffic_simulator_msgs::Vertex;
260 
261 auto toROS2Message(const traffic_simulator_msgs::Vertex &) -> traffic_simulator_msgs::msg::Vertex;
262 
263 auto toProtobufMessage(const traffic_simulator_msgs::msg::Polyline &)
264  -> traffic_simulator_msgs::Polyline;
265 
266 auto toROS2Message(const traffic_simulator_msgs::Polyline &)
267  -> traffic_simulator_msgs::msg::Polyline;
268 
269 auto toProtobufMessage(const traffic_simulator_msgs::msg::PolylineTrajectory &)
270  -> traffic_simulator_msgs::PolylineTrajectory;
271 
272 auto toROS2Message(const traffic_simulator_msgs::PolylineTrajectory &)
273  -> traffic_simulator_msgs::msg::PolylineTrajectory;
274 } // namespace simulation_interface
275 
276 #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
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:22
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:23