15 #ifndef TRAFFIC_SIMULATOR__API__API_HPP_
16 #define TRAFFIC_SIMULATOR__API__API_HPP_
18 #include <simulation_api_schema.pb.h>
22 #include <std_msgs/msg/float64.hpp>
29 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
49 template <
typename NodeT,
typename AllocatorT = std::allocator<
void>,
typename... Ts>
51 : configuration_(configuration),
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>())),
60 common::getParameter<bool>(node_parameters_,
"use_sim_time"),
61 std::forward<decltype(
xs)>(
xs)...),
64 common::getParameter<int>(node_parameters_,
"port", 5555)),
66 std::make_shared<entity::EntityManager>(node, configuration, node_parameters_)),
67 traffic_controller_ptr_(
std::make_shared<traffic::TrafficController>(
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) {
80 entity_manager_ptr_->setVerbose(configuration_.
verbose);
81 entity_manager_ptr_->setTrafficLights(traffic_lights_ptr_);
88 template <
typename ParameterT,
typename... Ts>
91 return common::getParameter<ParameterT>(node_parameters_, std::forward<Ts>(
xs)...);
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>>>>
119 const
std::
string & name, const PoseType & pose, const ParamsType & parameters,
120 const
std::
string & behavior = "", const
std::
string & model3d = "") -> entity::EntityBase &
122 using VehicleParameters = traffic_simulator_msgs::msg::VehicleParameters;
123 using PedestrianParameters = traffic_simulator_msgs::msg::PedestrianParameters;
124 using MiscObjectParameters = traffic_simulator_msgs::msg::MiscObjectParameters;
127 if constexpr (std::is_same_v<ParamsType, VehicleParameters>) {
130 name, pose, parameters,
getCurrentTime(), configuration_, node_parameters_);
136 }
else if constexpr (std::is_same_v<ParamsType, PedestrianParameters>) {
140 }
else if constexpr (std::is_same_v<ParamsType, MiscObjectParameters>) {
146 auto prepare_and_send_request = [&](
const auto & entity,
auto & request) ->
bool {
148 request.mutable_parameters()->set_name(name);
149 request.set_asset_key(model3d);
151 return zeromq_client_.
call(request).result().success();
154 auto register_to_environment_simulator = [&](
const auto & entity) ->
bool {
158 if constexpr (std::is_same_v<ParamsType, VehicleParameters>) {
159 simulation_api_schema::SpawnVehicleEntityRequest request;
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);
176 auto & entity = register_to_entity_manager();
177 if (register_to_environment_simulator(entity)) {
186 const std::string &,
const simulation_api_schema::ImuSensorConfiguration & configuration)
190 const simulation_api_schema::PseudoTrafficLightDetectorConfiguration &) -> bool;
192 auto attachLidarSensor(
const simulation_api_schema::LidarConfiguration &) -> bool;
195 const std::string &,
const double lidar_sensor_delay,
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;
213 auto
getEgoEntity(const
std::
string & name) const -> const entity::EgoEntity &;
220 auto
getEntity(const
std::
string & name) -> entity::EntityBase &;
222 auto
getEntity(const
std::
string & name) const -> const entity::EntityBase &;
231 const
std::
string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose,
232 const geometry_msgs::msg::
PoseStamped & goal_pose) ->
void;
234 auto
despawn(const
std::
string & name) ->
bool;
240 const
std::
string & first_entity_name, const
std::
string & second_entity_name) const ->
bool;
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;
274 auto updateTimeInSim() ->
bool;
276 auto updateEntitiesStatusInSim() ->
bool;
278 auto updateTrafficLightsInSim() ->
bool;
282 const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
284 const rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_pub_;
286 const rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
290 zeromq::MultiClient zeromq_client_;
292 const
std::shared_ptr<entity::EntityManager> entity_manager_ptr_;
294 const
std::shared_ptr<traffic::TrafficController> traffic_controller_ptr_;
298 const rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr real_time_factor_subscriber_;
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 ¶meters, 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: 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: 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
Definition: pedestrian_entity.hpp:37
static auto defaultBehavior() noexcept -> const std::string &
Definition: pedestrian_entity.hpp:56
Definition: vehicle_entity.hpp:40
static auto defaultBehavior() -> const std::string &
Definition: vehicle_entity.hpp:59
class definition for the traffic controller