scenario_simulator_v2 C++ API
api.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 TRAFFIC_SIMULATOR__API__API_HPP_
16 #define TRAFFIC_SIMULATOR__API__API_HPP_
17 
18 #include <simulation_api_schema.pb.h>
19 
22 #include <std_msgs/msg/float64.hpp>
29 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
30 
32 {
34 {
35  static auto autoware() noexcept -> const std::string &
36  {
37  static const std::string name = "Autoware";
38  return name;
39  }
40 };
41 
43 {
44 };
45 
46 class API
47 {
48 public:
49  template <typename NodeT, typename AllocatorT = std::allocator<void>, typename... Ts>
50  explicit API(NodeT && node, const Configuration & configuration, Ts &&... xs)
51  : configuration_(configuration),
52  node_parameters_(
53  rclcpp::node_interfaces::get_node_parameters_interface(std::forward<NodeT>(node))),
54  clock_pub_(rclcpp::create_publisher<rosgraph_msgs::msg::Clock>(
55  node, "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(),
56  rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
57  debug_marker_pub_(rclcpp::create_publisher<visualization_msgs::msg::MarkerArray>(
58  node, "debug_marker", rclcpp::QoS(100), rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
59  clock_(
60  common::getParameter<bool>(node_parameters_, "use_sim_time"),
61  std::forward<decltype(xs)>(xs)...),
62  zeromq_client_(
63  simulation_interface::protocol, configuration.simulator_host,
64  common::getParameter<int>(node_parameters_, "port", 5555)),
65  entity_manager_ptr_(
66  std::make_shared<entity::EntityManager>(node, configuration, node_parameters_)),
67  traffic_controller_ptr_(std::make_shared<traffic::TrafficController>(
68  [this](const std::string & name) { despawn(name); }, entity_manager_ptr_,
69  configuration.auto_sink_entity_types)),
70  traffic_lights_ptr_(std::make_shared<TrafficLights>(
71  node, entity_manager_ptr_->getHdmapUtils(),
72  common::getParameter<std::string>(
73  node_parameters_, "architecture_type", "awf/universe/20240605"))),
74  real_time_factor_subscriber_(rclcpp::create_subscription<std_msgs::msg::Float64>(
75  node, "/real_time_factor", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(),
76  [this](const std_msgs::msg::Float64 & message) {
77  return setSimulationStepTime(message.data);
78  }))
79  {
80  entity_manager_ptr_->setVerbose(configuration_.verbose);
81  entity_manager_ptr_->setTrafficLights(traffic_lights_ptr_);
82  if (not init()) {
83  throw common::SimulationError("Failed to initialize simulator by InitializeRequest");
84  }
85  }
86 
87  // global
88  template <typename ParameterT, typename... Ts>
89  auto getROS2Parameter(Ts &&... xs) const -> decltype(auto)
90  {
91  return common::getParameter<ParameterT>(node_parameters_, std::forward<Ts>(xs)...);
92  }
93 
94  auto init() -> bool;
95 
96  auto setVerbose(const bool verbose) -> void;
97 
98  auto setSimulationStepTime(const double step_time) -> bool;
99 
100  auto startNpcLogic() -> void;
101 
102  auto isNpcLogicStarted() const -> bool;
103 
104  auto getCurrentTime() const noexcept -> double;
105 
106  auto closeZMQConnection() -> void;
107 
108  // update
109  auto updateFrame() -> bool;
110 
111  // entities, ego - spawn
112  template <
113  typename PoseType, typename ParamsType,
114  typename = std::enable_if_t<std::disjunction_v<
115  std::is_same<std::decay_t<ParamsType>, traffic_simulator_msgs::msg::VehicleParameters>,
116  std::is_same<std::decay_t<ParamsType>, traffic_simulator_msgs::msg::PedestrianParameters>,
117  std::is_same<std::decay_t<ParamsType>, traffic_simulator_msgs::msg::MiscObjectParameters>>>>
118  auto spawn(
119  const std::string & name, const PoseType & pose, const ParamsType & parameters,
120  const std::string & behavior = "", const std::string & model3d = "") -> entity::EntityBase &
121  {
122  using VehicleParameters = traffic_simulator_msgs::msg::VehicleParameters;
123  using PedestrianParameters = traffic_simulator_msgs::msg::PedestrianParameters;
124  using MiscObjectParameters = traffic_simulator_msgs::msg::MiscObjectParameters;
125 
126  auto register_to_entity_manager = [&]() -> entity::EntityBase & {
127  if constexpr (std::is_same_v<ParamsType, VehicleParameters>) {
128  if (behavior == VehicleBehavior::autoware()) {
129  return entity_manager_ptr_->spawnEntity<entity::EgoEntity>(
130  name, pose, parameters, getCurrentTime(), configuration_, node_parameters_);
131  } else {
132  return entity_manager_ptr_->spawnEntity<entity::VehicleEntity>(
133  name, pose, parameters, getCurrentTime(),
134  behavior.empty() ? VehicleBehavior::defaultBehavior() : behavior);
135  }
136  } else if constexpr (std::is_same_v<ParamsType, PedestrianParameters>) {
137  return entity_manager_ptr_->spawnEntity<entity::PedestrianEntity>(
138  name, pose, parameters, getCurrentTime(),
139  behavior.empty() ? PedestrianBehavior::defaultBehavior() : behavior);
140  } else if constexpr (std::is_same_v<ParamsType, MiscObjectParameters>) {
141  return entity_manager_ptr_->spawnEntity<entity::MiscObjectEntity>(
142  name, pose, parameters, getCurrentTime());
143  }
144  };
145 
146  auto prepare_and_send_request = [&](const auto & entity, auto & request) -> bool {
147  simulation_interface::toProto(parameters, *request.mutable_parameters());
148  request.mutable_parameters()->set_name(name);
149  request.set_asset_key(model3d);
150  simulation_interface::toProto(entity.getMapPose(), *request.mutable_pose());
151  return zeromq_client_.call(request).result().success();
152  };
153 
154  auto register_to_environment_simulator = [&](const auto & entity) -> bool {
155  if (configuration_.standalone_mode) {
156  return true;
157  } else {
158  if constexpr (std::is_same_v<ParamsType, VehicleParameters>) {
159  simulation_api_schema::SpawnVehicleEntityRequest request;
160  request.set_is_ego(behavior == VehicleBehavior::autoware());
162  request.set_initial_speed(0.0);
163  return prepare_and_send_request(entity, request);
164  } else if constexpr (std::is_same_v<ParamsType, PedestrianParameters>) {
165  simulation_api_schema::SpawnPedestrianEntityRequest request;
166  return prepare_and_send_request(entity, request);
167  } else if constexpr (std::is_same_v<ParamsType, MiscObjectParameters>) {
168  simulation_api_schema::SpawnMiscObjectEntityRequest request;
169  return prepare_and_send_request(entity, request);
170  } else {
171  return false;
172  }
173  }
174  };
175 
176  auto & entity = register_to_entity_manager();
177  if (register_to_environment_simulator(entity)) {
178  return entity;
179  } else {
180  THROW_SEMANTIC_ERROR("Spawn entity ", std::quoted(name), " resulted in failure.");
181  }
182  }
183 
184  // sensors - attach
185  auto attachImuSensor(
186  const std::string &, const simulation_api_schema::ImuSensorConfiguration & configuration)
187  -> bool;
188 
190  const simulation_api_schema::PseudoTrafficLightDetectorConfiguration &) -> bool;
191 
192  auto attachLidarSensor(const simulation_api_schema::LidarConfiguration &) -> bool;
193 
194  auto attachLidarSensor(
195  const std::string &, const double lidar_sensor_delay,
197 
198  auto attachDetectionSensor(const simulation_api_schema::DetectionSensorConfiguration &) -> bool;
199 
201  const std::string &, double detection_sensor_range, bool detect_all_objects_in_range,
202  double pos_noise_stddev, int random_seed, double probability_of_lost,
203  double object_recognition_delay) -> bool;
204 
205  auto attachOccupancyGridSensor(const simulation_api_schema::OccupancyGridSensorConfiguration &)
206  -> bool;
207 
208  // ego - checks, getters
209  auto getFirstEgoName() const -> std::optional<std::string>;
210 
211  auto getEgoEntity(const std::string & name) -> entity::EgoEntity &;
212 
213  auto getEgoEntity(const std::string & name) const -> const entity::EgoEntity &;
214 
215  // entities - checks, getters
216  auto isEntityExist(const std::string & name) const -> bool;
217 
218  auto getEntityNames() const -> std::vector<std::string>;
219 
220  auto getEntity(const std::string & name) -> entity::EntityBase &;
221 
222  auto getEntity(const std::string & name) const -> const entity::EntityBase &;
223 
224  auto getEntityPointer(const std::string & name) const -> std::shared_ptr<entity::EntityBase>;
225 
226  // entities - respawn, despawn, reset
227  auto resetBehaviorPlugin(const std::string & name, const std::string & behavior_plugin_name)
228  -> void;
229 
230  auto respawn(
231  const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose,
232  const geometry_msgs::msg::PoseStamped & goal_pose) -> void;
233 
234  auto despawn(const std::string & name) -> bool;
235 
236  auto despawnEntities() -> bool;
237 
238  // entities - features
239  auto checkCollision(
240  const std::string & first_entity_name, const std::string & second_entity_name) const -> bool;
241 
242  // traffics, lanelet
243  auto getHdmapUtils() const -> const std::shared_ptr<hdmap_utils::HdMapUtils> &;
244 
245  auto getV2ITrafficLights() const -> std::shared_ptr<V2ITrafficLights>;
246 
247  auto getConventionalTrafficLights() const -> std::shared_ptr<ConventionalTrafficLights>;
266  auto addTrafficSource(
267  const double radius, const double rate, const double speed,
268  const geometry_msgs::msg::Pose & position,
269  const traffic::TrafficSource::Distribution & distribution,
270  const bool allow_spawn_outside_lane = false, const bool require_footprint_fitting = false,
271  const bool random_orientation = false, std::optional<int> random_seed = std::nullopt) -> void;
272 
273 private:
274  auto updateTimeInSim() -> bool;
275 
276  auto updateEntitiesStatusInSim() -> bool;
277 
278  auto updateTrafficLightsInSim() -> bool;
279 
280  const Configuration configuration_;
281 
282  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
283 
284  const rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_pub_;
285 
286  const rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
287 
288  SimulationClock clock_;
289 
290  zeromq::MultiClient zeromq_client_;
291 
292  const std::shared_ptr<entity::EntityManager> entity_manager_ptr_;
293 
294  const std::shared_ptr<traffic::TrafficController> traffic_controller_ptr_;
295 
296  const std::shared_ptr<TrafficLights> traffic_lights_ptr_;
297 
298  const rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr real_time_factor_subscriber_;
299 };
300 } // namespace traffic_simulator
301 
302 #endif // TRAFFIC_SIMULATOR__API__API_HPP_
Definition: api.hpp:47
auto isNpcLogicStarted() const -> bool
Definition: api.cpp:65
auto addTrafficSource(const double radius, const double rate, const double speed, const geometry_msgs::msg::Pose &position, const traffic::TrafficSource::Distribution &distribution, const bool allow_spawn_outside_lane=false, const bool require_footprint_fitting=false, const bool random_orientation=false, std::optional< int > random_seed=std::nullopt) -> void
Add a traffic source to the simulation.
Definition: api.cpp:376
auto checkCollision(const std::string &first_entity_name, const std::string &second_entity_name) const -> bool
Definition: api.cpp:344
API(NodeT &&node, const Configuration &configuration, Ts &&... xs)
Definition: api.hpp:50
auto setSimulationStepTime(const double step_time) -> bool
Definition: api.cpp:39
auto attachOccupancyGridSensor(const simulation_api_schema::OccupancyGridSensorConfiguration &) -> bool
Definition: api.cpp:216
auto getEntity(const std::string &name) -> entity::EntityBase &
Definition: api.cpp:255
auto closeZMQConnection() -> void
Definition: api.cpp:69
auto getCurrentTime() const noexcept -> double
Definition: api.cpp:67
auto attachPseudoTrafficLightDetector(const simulation_api_schema::PseudoTrafficLightDetectorConfiguration &) -> bool
Definition: api.cpp:163
auto setVerbose(const bool verbose) -> void
Definition: api.cpp:37
auto getFirstEgoName() const -> std::optional< std::string >
Definition: api.cpp:229
auto getEntityNames() const -> std::vector< std::string >
Definition: api.cpp:250
auto getEntityPointer(const std::string &name) const -> std::shared_ptr< entity::EntityBase >
Definition: api.cpp:265
auto resetBehaviorPlugin(const std::string &name, const std::string &behavior_plugin_name) -> void
Definition: api.cpp:271
auto getV2ITrafficLights() const -> std::shared_ptr< V2ITrafficLights >
Definition: api.cpp:366
auto getROS2Parameter(Ts &&... xs) const -> decltype(auto)
Definition: api.hpp:89
auto init() -> bool
Definition: api.cpp:21
auto getHdmapUtils() const -> const std::shared_ptr< hdmap_utils::HdMapUtils > &
Definition: api.cpp:361
auto startNpcLogic() -> void
Definition: api.cpp:55
auto attachLidarSensor(const simulation_api_schema::LidarConfiguration &) -> bool
Definition: api.cpp:171
auto spawn(const std::string &name, const PoseType &pose, const ParamsType &parameters, const std::string &behavior="", const std::string &model3d="") -> entity::EntityBase &
Definition: api.hpp:118
auto attachDetectionSensor(const simulation_api_schema::DetectionSensorConfiguration &) -> bool
Definition: api.cpp:193
auto respawn(const std::string &name, const geometry_msgs::msg::PoseWithCovarianceStamped &new_pose, const geometry_msgs::msg::PoseStamped &goal_pose) -> void
Definition: api.cpp:277
auto getConventionalTrafficLights() const -> std::shared_ptr< ConventionalTrafficLights >
Definition: api.cpp:371
auto getEgoEntity(const std::string &name) -> entity::EgoEntity &
Definition: api.cpp:234
auto updateFrame() -> bool
Definition: api.cpp:127
auto despawnEntities() -> bool
Definition: api.cpp:336
auto attachImuSensor(const std::string &, const simulation_api_schema::ImuSensorConfiguration &configuration) -> bool
Definition: api.cpp:155
auto despawn(const std::string &name) -> bool
Definition: api.cpp:322
auto isEntityExist(const std::string &name) const -> bool
Definition: api.cpp:245
Definition: traffic_lights.hpp:33
Definition: simulation_clock.hpp:24
Definition: traffic_lights.hpp:132
Definition: traffic_lights.hpp:62
Definition: ego_entity.hpp:35
Definition: entity_base.hpp:51
Definition: misc_object_entity.hpp:27
Definition: pedestrian_entity.hpp:34
Definition: vehicle_entity.hpp:37
auto call(const simulation_api_schema::SimulationRequest &) -> simulation_api_schema::SimulationResponse
Definition: zmq_multi_client.cpp:43
#define THROW_SEMANTIC_ERROR(...)
Definition: exception.hpp:59
Definition: concatenate.hpp:24
Definition: cache.hpp:28
Definition: constants.hpp:21
void toProto(const geometry_msgs::msg::Point &p, geometry_msgs::Point &proto)
Definition: conversions.cpp:22
const TransportProtocol protocol
Definition: constants.hpp:30
Definition: lanelet_wrapper.hpp:40
LidarType
Definition: helper.hpp:141
geometry_msgs::msg::Pose Pose
Definition: lanelet_map.hpp:30
geometry_msgs::msg::PoseStamped PoseStamped
Definition: lanelet_wrapper.hpp:67
Definition: api.hpp:32
Definition: junit5.hpp:25
std::string string
Definition: junit5.hpp:26
Definition: conversions.hpp:56
A problem occurred that interfered with the continuation of the simulation.
Definition: exception.hpp:47
Definition: configuration.hpp:31
bool verbose
Definition: configuration.hpp:36
const bool standalone_mode
Definition: configuration.hpp:40
static auto autoware() noexcept -> const std::string &
Definition: api.hpp:35
static auto defaultBehavior() noexcept -> const std::string &
Definition: pedestrian_entity.hpp:56
static auto defaultBehavior() -> const std::string &
Definition: vehicle_entity.hpp:59
class definition for the traffic controller