15 #ifndef TRAFFIC_SIMULATOR__API__API_HPP_
16 #define TRAFFIC_SIMULATOR__API__API_HPP_
18 #include <simulation_api_schema.pb.h>
20 #include <autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp>
21 #include <autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp>
22 #include <boost/variant.hpp>
26 #include <rclcpp/rclcpp.hpp>
27 #include <rosgraph_msgs/msg/clock.hpp>
30 #include <std_msgs/msg/float64.hpp>
44 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
65 template <
typename NodeT,
typename AllocatorT = std::allocator<
void>,
typename... Ts>
67 : configuration(configuration),
69 rclcpp::node_interfaces::get_node_parameters_interface(
std::forward<NodeT>(node))),
71 std::make_shared<entity::EntityManager>(node, configuration, node_parameters_)),
72 traffic_controller_ptr_(
std::make_shared<traffic::TrafficController>(
75 [
this](
const auto & name) {
return API::despawn(name); }, configuration.auto_sink)),
76 clock_pub_(rclcpp::create_publisher<rosgraph_msgs::msg::Clock>(
77 node,
"/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(),
78 rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
79 debug_marker_pub_(rclcpp::create_publisher<visualization_msgs::msg::MarkerArray>(
80 node,
"debug_marker", rclcpp::QoS(100), rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
81 real_time_factor_subscriber(rclcpp::create_subscription<std_msgs::msg::Float64>(
82 node,
"/real_time_factor", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(),
83 [
this](
const std_msgs::msg::Float64 & message) {
88 if (message.data >= 0.001) {
89 clock_.realtime_factor = message.data;
90 simulation_api_schema::UpdateStepTimeRequest request;
91 request.set_simulation_step_time(clock_.getStepTime());
92 zeromq_client_.call(request);
95 clock_(node->get_parameter(
"use_sim_time").as_bool(), std::forward<decltype(
xs)>(
xs)...),
102 simulation_api_schema::InitializeRequest request;
109 if (not zeromq_client_.
call(request).result().success()) {
115 template <
typename Node>
118 if (!node.has_parameter(
"port")) node.declare_parameter(
"port", 5555);
119 return node.get_parameter(
"port").as_int();
126 template <
typename Pose>
129 const traffic_simulator_msgs::msg::VehicleParameters & parameters,
133 auto register_to_entity_manager = [&]() {
135 return entity_manager_ptr_->entityExists(name) or
137 name, pose, parameters, configuration, node_parameters_);
140 name, pose, parameters, behavior);
144 auto register_to_environment_simulator = [&]() {
148 simulation_api_schema::SpawnVehicleEntityRequest req;
150 req.mutable_parameters()->set_name(name);
151 req.set_asset_key(model3d);
155 req.set_initial_speed(0.0);
156 return zeromq_client_.
call(req).result().success();
160 return register_to_entity_manager() and register_to_environment_simulator();
163 geometry_msgs::msg::Pose
toMapPose(
const geometry_msgs::msg::Pose & pose) {
return pose; }
167 return entity_manager_ptr_->getHdmapUtils()->toMapPose(pose).pose;
170 template <
typename Pose>
173 const traffic_simulator_msgs::msg::PedestrianParameters & parameters,
177 auto register_to_entity_manager = [&]() {
179 name, pose, parameters, behavior);
182 auto register_to_environment_simulator = [&]() {
186 simulation_api_schema::SpawnPedestrianEntityRequest req;
188 req.mutable_parameters()->set_name(name);
189 req.set_asset_key(model3d);
191 return zeromq_client_.
call(req).result().success();
195 return register_to_entity_manager() and register_to_environment_simulator();
198 template <
typename Pose>
201 const traffic_simulator_msgs::msg::MiscObjectParameters & parameters,
204 auto register_to_entity_manager = [&]() {
208 auto register_to_environment_simulator = [&]() {
212 simulation_api_schema::SpawnMiscObjectEntityRequest req;
214 req.mutable_parameters()->set_name(name);
215 req.set_asset_key(model3d);
217 return zeromq_client_.
call(req).result().success();
221 return register_to_entity_manager() and register_to_environment_simulator();
228 const std::string & name,
const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose,
229 const geometry_msgs::msg::PoseStamped & goal_pose) -> void;
233 const std::string & name,
const geometry_msgs::msg::Pose & map_pose,
234 const traffic_simulator_msgs::msg::ActionStatus & action_status =
238 const traffic_simulator_msgs::msg::ActionStatus & action_status =
242 const geometry_msgs::msg::Pose & relative_pose,
243 const traffic_simulator_msgs::msg::ActionStatus & action_status =
247 const geometry_msgs::msg::Point & relative_position,
248 const geometry_msgs::msg::Vector3 & relative_rpy,
249 const traffic_simulator_msgs::msg::ActionStatus & action_status =
255 const simulation_api_schema::PseudoTrafficLightDetectorConfiguration &);
259 const std::string &,
const double lidar_sensor_delay,
264 const std::string &,
double detection_sensor_range,
bool detect_all_objects_in_range,
265 double pos_noise_stddev,
int random_seed,
double probability_of_lost,
266 double object_recognition_delay);
311 const double radius,
const double rate,
const double speed,
312 const geometry_msgs::msg::Pose & position,
314 const bool allow_spawn_outside_lane =
false,
const bool require_footprint_fitting =
false,
315 const bool random_orientation =
false, std::optional<int> random_seed = std::nullopt) -> void;
318 #define FORWARD_TO_ENTITY_MANAGER(NAME) \
324 template <typename... Ts> \
325 decltype(auto) NAME(Ts &&... xs) \
327 assert(entity_manager_ptr_); \
328 return (*entity_manager_ptr_).NAME(std::forward<decltype(xs)>(xs)...); \
330 static_assert(true, "")
392 #undef FORWARD_TO_ENTITY_MANAGER
394 template <
typename ParameterT,
typename... Ts>
397 return getParameter<ParameterT>(node_parameters_, std::forward<Ts>(
xs)...);
405 auto toLaneletPose(
const geometry_msgs::msg::Pose & map_pose,
bool include_crosswalk)
const
406 -> std::optional<CanonicalizedLaneletPose>;
409 bool updateTimeInSim();
411 bool updateEntitiesStatusInSim();
413 bool updateTrafficLightsInSim();
417 const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
419 const std::shared_ptr<entity::EntityManager> entity_manager_ptr_;
421 const std::shared_ptr<traffic::TrafficController> traffic_controller_ptr_;
423 const rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_pub_;
425 const rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
427 const rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr real_time_factor_subscriber;
#define FORWARD_TO_ENTITY_MANAGER(NAME)
Definition: api.hpp:318
auto spawn(const std::string &name, const Pose &pose, const traffic_simulator_msgs::msg::MiscObjectParameters ¶meters, const std::string &model3d="")
Definition: api.hpp:199
decltype(auto) setConventionalTrafficLightConfidence(Ts &&... xs)
Forward to arguments to the EntityManager:: setConventionalTrafficLightConfidence function.
Definition: api.hpp:375
decltype(auto) getHdmapUtils(Ts &&... xs)
Forward to arguments to the EntityManager:: getHdmapUtils function.
Definition: api.hpp:346
decltype(auto) requestFollowTrajectory(Ts &&... xs)
Forward to arguments to the EntityManager:: requestFollowTrajectory function.
Definition: api.hpp:363
double getCurrentTime() const noexcept
Definition: api.hpp:272
decltype(auto) resetBehaviorPlugin(Ts &&... xs)
Forward to arguments to the EntityManager:: resetBehaviorPlugin function.
Definition: api.hpp:368
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:379
API(NodeT &&node, const Configuration &configuration, Ts &&... xs)
Definition: api.hpp:66
decltype(auto) isEgoSpawned(Ts &&... xs)
Forward to arguments to the EntityManager:: isEgoSpawned function.
Definition: api.hpp:357
decltype(auto) getTraveledDistance(Ts &&... xs)
Forward to arguments to the EntityManager:: getTraveledDistance function.
Definition: api.hpp:353
void requestLaneChange(const std::string &name, const lanelet::Id &lanelet_id)
Definition: api.cpp:346
decltype(auto) setMapPose(Ts &&... xs)
Forward to arguments to the EntityManager:: setMapPose function.
Definition: api.hpp:379
decltype(auto) setBehaviorParameter(Ts &&... xs)
Forward to arguments to the EntityManager:: setBehaviorParameter function.
Definition: api.hpp:374
decltype(auto) getConventionalTrafficLight(Ts &&... xs)
Forward to arguments to the EntityManager:: getConventionalTrafficLight function.
Definition: api.hpp:336
void startNpcLogic()
Definition: api.cpp:337
decltype(auto) isInLanelet(Ts &&... xs)
Forward to arguments to the EntityManager:: isInLanelet function.
Definition: api.hpp:358
decltype(auto) setDecelerationRateLimit(Ts &&... xs)
Forward to arguments to the EntityManager:: setDecelerationRateLimit function.
Definition: api.hpp:377
decltype(auto) requestWalkStraight(Ts &&... xs)
Forward to arguments to the EntityManager:: requestWalkStraight function.
Definition: api.hpp:366
int getZMQSocketPort(Node &node)
Definition: api.hpp:116
decltype(auto) getBoundingBox(Ts &&... xs)
Forward to arguments to the EntityManager:: getBoundingBox function.
Definition: api.hpp:335
bool attachOccupancyGridSensor(const simulation_api_schema::OccupancyGridSensorConfiguration &)
Definition: api.cpp:226
decltype(auto) resetConventionalTrafficLightPublishRate(Ts &&... xs)
Forward to arguments to the EntityManager:: resetConventionalTrafficLightPublishRate function.
Definition: api.hpp:369
decltype(auto) getStandStillDuration(Ts &&... xs)
Forward to arguments to the EntityManager:: getStandStillDuration function.
Definition: api.hpp:352
decltype(auto) requestSynchronize(Ts &&... xs)
Forward to arguments to the EntityManager:: requestSynchronize function.
Definition: api.hpp:365
decltype(auto) setAccelerationLimit(Ts &&... xs)
Forward to arguments to the EntityManager:: setAccelerationLimit function.
Definition: api.hpp:372
decltype(auto) resetV2ITrafficLightPublishRate(Ts &&... xs)
Forward to arguments to the EntityManager:: resetV2ITrafficLightPublishRate function.
Definition: api.hpp:370
decltype(auto) requestClearRoute(Ts &&... xs)
Forward to arguments to the EntityManager:: requestClearRoute function.
Definition: api.hpp:367
void setVerbose(const bool verbose)
Definition: api.cpp:31
decltype(auto) checkCollision(Ts &&... xs)
Forward to arguments to the EntityManager:: checkCollision function.
Definition: api.hpp:332
decltype(auto) reachPosition(Ts &&... xs)
Forward to arguments to the EntityManager:: reachPosition function.
Definition: api.hpp:356
decltype(auto) setAcceleration(Ts &&... xs)
Forward to arguments to the EntityManager:: setAcceleration function.
Definition: api.hpp:371
decltype(auto) getCurrentAccel(Ts &&... xs)
Forward to arguments to the EntityManager:: getCurrentAccel function.
Definition: api.hpp:338
auto getROS2Parameter(Ts &&... xs) const -> decltype(auto)
Definition: api.hpp:391
auto spawn(const std::string &name, const Pose &pose, const traffic_simulator_msgs::msg::VehicleParameters ¶meters, const std::string &behavior=VehicleBehavior::defaultBehavior(), const std::string &model3d="")
Definition: api.hpp:127
decltype(auto) getLinearJerk(Ts &&... xs)
Forward to arguments to the EntityManager:: getLinearJerk function.
Definition: api.hpp:349
decltype(auto) getMapPoseFromRelativePose(Ts &&... xs)
Forward to arguments to the EntityManager:: getMapPoseFromRelativePose function.
Definition: api.hpp:351
decltype(auto) getEntityStatusBeforeUpdate(Ts &&... xs)
Forward to arguments to the EntityManager:: getEntityStatusBeforeUpdate function.
Definition: api.hpp:345
decltype(auto) getEntityStatus(Ts &&... xs)
Forward to arguments to the EntityManager:: getEntityStatus function.
Definition: api.hpp:344
bool attachDetectionSensor(const simulation_api_schema::DetectionSensorConfiguration &)
Definition: api.cpp:203
decltype(auto) getV2ITrafficLights(Ts &&... xs)
Forward to arguments to the EntityManager:: getV2ITrafficLights function.
Definition: api.hpp:355
std::optional< double > getTimeHeadway(const std::string &from, const std::string &to)
Definition: api.cpp:143
auto setEntityStatus(const std::string &name, const CanonicalizedEntityStatus &) -> void
Definition: api.cpp:102
auto canonicalize(const LaneletPose &maybe_non_canonicalized_lanelet_pose) const -> CanonicalizedLaneletPose
Definition: api.cpp:398
decltype(auto) getConventionalTrafficLights(Ts &&... xs)
Forward to arguments to the EntityManager:: getConventionalTrafficLights function.
Definition: api.hpp:337
decltype(auto) setTwist(Ts &&... xs)
Forward to arguments to the EntityManager:: setTwist function.
Definition: api.hpp:380
decltype(auto) isNpcLogicStarted(Ts &&... xs)
Forward to arguments to the EntityManager:: isNpcLogicStarted function.
Definition: api.hpp:359
decltype(auto) getCurrentAction(Ts &&... xs)
Forward to arguments to the EntityManager:: getCurrentAction function.
Definition: api.hpp:339
decltype(auto) getV2ITrafficLight(Ts &&... xs)
Forward to arguments to the EntityManager:: getV2ITrafficLight function.
Definition: api.hpp:354
decltype(auto) getLaneletPose(Ts &&... xs)
Forward to arguments to the EntityManager:: getLaneletPose function.
Definition: api.hpp:348
auto spawn(const std::string &name, const Pose &pose, const traffic_simulator_msgs::msg::PedestrianParameters ¶meters, const std::string &behavior=PedestrianBehavior::defaultBehavior(), const std::string &model3d="")
Definition: api.hpp:171
geometry_msgs::msg::Pose toMapPose(const geometry_msgs::msg::Pose &pose)
Definition: api.hpp:163
auto respawn(const std::string &name, const geometry_msgs::msg::PoseWithCovarianceStamped &new_pose, const geometry_msgs::msg::PoseStamped &goal_pose) -> void
Definition: api.cpp:54
decltype(auto) laneMatchingSucceed(Ts &&... xs)
Forward to arguments to the EntityManager:: laneMatchingSucceed function.
Definition: api.hpp:360
void closeZMQConnection()
Definition: api.hpp:122
decltype(auto) getEntityNames(Ts &&... xs)
Forward to arguments to the EntityManager:: getEntityNames function.
Definition: api.hpp:343
geometry_msgs::msg::Pose toMapPose(const traffic_simulator_msgs::msg::LaneletPose &pose)
Definition: api.hpp:165
decltype(auto) setAccelerationRateLimit(Ts &&... xs)
Forward to arguments to the EntityManager:: setAccelerationRateLimit function.
Definition: api.hpp:373
bool attachPseudoTrafficLightDetector(const simulation_api_schema::PseudoTrafficLightDetectorConfiguration &)
Definition: api.cpp:195
decltype(auto) requestSpeedChange(Ts &&... xs)
Forward to arguments to the EntityManager:: requestSpeedChange function.
Definition: api.hpp:364
decltype(auto) setLinearVelocity(Ts &&... xs)
Forward to arguments to the EntityManager:: setLinearVelocity function.
Definition: api.hpp:378
decltype(auto) cancelRequest(Ts &&... xs)
Forward to arguments to the EntityManager:: cancelRequest function.
Definition: api.hpp:331
decltype(auto) setDecelerationLimit(Ts &&... xs)
Forward to arguments to the EntityManager:: setDecelerationLimit function.
Definition: api.hpp:376
bool despawnEntities()
Definition: api.cpp:47
decltype(auto) setVelocityLimit(Ts &&... xs)
Forward to arguments to the EntityManager:: setVelocityLimit function.
Definition: api.hpp:381
decltype(auto) entityExists(Ts &&... xs)
Forward to arguments to the EntityManager:: entityExists function.
Definition: api.hpp:333
decltype(auto) getEntity(Ts &&... xs)
Forward to arguments to the EntityManager:: getEntity function.
Definition: api.hpp:342
bool despawn(const std::string &name)
Definition: api.cpp:33
decltype(auto) getCurrentTwist(Ts &&... xs)
Forward to arguments to the EntityManager:: getCurrentTwist function.
Definition: api.hpp:340
decltype(auto) activateOutOfRangeJob(Ts &&... xs)
Forward to arguments to the EntityManager:: activateOutOfRangeJob function.
Definition: api.hpp:329
bool attachLidarSensor(const simulation_api_schema::LidarConfiguration &)
Definition: api.cpp:238
decltype(auto) getEgoName(Ts &&... xs)
Forward to arguments to the EntityManager:: getEgoName function.
Definition: api.hpp:341
decltype(auto) getMapPose(Ts &&... xs)
Forward to arguments to the EntityManager:: getMapPose function.
Definition: api.hpp:350
decltype(auto) asFieldOperatorApplication(Ts &&... xs)
Forward to arguments to the EntityManager:: asFieldOperatorApplication function.
Definition: api.hpp:330
auto toLaneletPose(const geometry_msgs::msg::Pose &map_pose, bool include_crosswalk) const -> std::optional< CanonicalizedLaneletPose >
Definition: api.cpp:412
decltype(auto) getLaneletLength(Ts &&... xs)
Forward to arguments to the EntityManager:: getLaneletLength function.
Definition: api.hpp:347
decltype(auto) requestAssignRoute(Ts &&... xs)
Forward to arguments to the EntityManager:: requestAssignRoute function.
Definition: api.hpp:362
decltype(auto) getBehaviorParameter(Ts &&... xs)
Forward to arguments to the EntityManager:: getBehaviorParameter function.
Definition: api.hpp:334
bool updateFrame()
Definition: api.cpp:311
decltype(auto) requestAcquirePosition(Ts &&... xs)
Forward to arguments to the EntityManager:: requestAcquirePosition function.
Definition: api.hpp:361
Definition: simulation_clock.hpp:24
auto getCurrentRosTime() -> rclcpp::Time
Definition: simulation_clock.cpp:41
double realtime_factor
Definition: simulation_clock.hpp:49
auto getCurrentScenarioTime() const
Definition: simulation_clock.hpp:32
auto getCurrentSimulationTime() const
Definition: simulation_clock.hpp:37
auto getStepTime() const
Definition: simulation_clock.hpp:39
Definition: ego_entity.hpp:37
Definition: misc_object_entity.hpp:27
Definition: pedestrian_entity.hpp:34
Definition: vehicle_entity.hpp:37
Definition: entity_status.hpp:29
Definition: lanelet_pose.hpp:27
std::vector< std::tuple< VehicleOrPedestrianParameter, std::string, std::string, double > > Distribution
Definition: traffic_source.hpp:69
Definition: zmq_multi_client.hpp:33
void closeConnection()
Definition: zmq_multi_client.cpp:33
auto call(const simulation_api_schema::SimulationRequest &) -> simulation_api_schema::SimulationResponse
Definition: zmq_multi_client.cpp:43
void toProto(const geometry_msgs::msg::Point &p, geometry_msgs::Point &proto)
Definition: conversions.cpp:22
const TransportProtocol protocol
Definition: constants.hpp:30
traffic_simulator_msgs::msg::ActionStatus constructActionStatus(double linear_vel=0, double angular_vel=0, double linear_accel=0, double angular_accel=0)
helper function for constructing action status
Definition: helper.cpp:24
LidarType
Definition: helper.hpp:116
TrajectoryShape
Definition: lane_change.hpp:31
Direction
Definition: lane_change.hpp:29
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:22
traffic_simulator_msgs::msg::EntityStatus EntityStatus
Definition: entity_status.hpp:24
Definition: junit5.hpp:25
std::string string
Definition: junit5.hpp:26
A problem occurred that interfered with the continuation of the simulation.
Definition: exception.hpp:47
Definition: configuration.hpp:30
auto lanelet2_map_path() const
Definition: configuration.hpp:138
bool standalone_mode
Definition: configuration.hpp:39
bool verbose
Definition: configuration.hpp:37
static auto autoware() noexcept -> const std::string &
Definition: api.hpp:51
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
Definition: lane_change.hpp:34
Definition: lane_change.hpp:44
parameters for behavior plugin
Definition: lane_change.hpp:75
Definition: lane_change.hpp:58
class definition for the traffic controller