15 #ifndef OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_
16 #define OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_
43 static inline std::unique_ptr<traffic_simulator::API> core =
nullptr;
46 template <
typename Node,
typename... Ts>
51 core = std::make_unique<traffic_simulator::API>(
52 node, configuration, std::forward<decltype(
xs)>(
xs)...);
54 throw Error(
"The simulator core has already been instantiated.");
58 static auto active() {
return static_cast<bool>(core); }
63 core->despawnEntities();
64 core->closeZMQConnection();
69 static auto update() ->
void { core->updateFrame(); }
80 template <
typename T,
typename std::enable_if_t<std::is_same_v<T, NativeLanePosition>,
int> = 0>
83 constexpr
bool include_crosswalk{
false};
87 return result.value();
90 "The specified WorldPosition = [", pose.position.x,
", ", pose.position.y,
", ",
92 "] could not be approximated to the proper Lane. Perhaps the "
93 "WorldPosition points to a location where multiple lanes overlap, and "
94 "there are at least two or more candidates for a LanePosition that "
95 "can be approximated to that WorldPosition. This issue can be "
96 "resolved by strictly specifying the location using LanePosition "
97 "instead of WorldPosition");
102 typename T,
typename std::enable_if_t<std::is_same_v<T, NativeWorldPosition>,
int> = 0>
111 if (
const auto from_entity = core->getEntityPointer(from_entity_name)) {
112 if (
const auto to_entity = core->getEntityPointer(to_entity_name)) {
115 from_entity->getMapPose(), to_entity->getMapPose()))
116 return relative_pose.value();
125 if (
const auto from_entity = core->getEntityPointer(from_entity_name)) {
127 const auto relative_pose =
129 return relative_pose.value();
138 if (
const auto to_entity = core->getEntityPointer(to_entity_name)) {
140 const auto relative_pose =
142 return relative_pose.value();
150 const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined)
153 if (
const auto to_entity = core->getEntityPointer(to_entity_name)) {
154 if (
const auto to_lanelet_pose = to_entity->getCanonicalizedLaneletPose()) {
156 from_entity_name, to_lanelet_pose.value(), routing_algorithm);
164 const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined)
167 if (
const auto from_entity = core->getEntityPointer(from_entity_name)) {
168 if (
const auto from_lanelet_pose = from_entity->getCanonicalizedLaneletPose()) {
170 from_lanelet_pose.value(), to_lanelet_pose, routing_algorithm);
178 const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined)
183 (routing_algorithm == RoutingAlgorithm::value_type::shortest);
185 from_lanelet_pose, to_lanelet_pose, routing_configuration);
190 const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined)
192 if (
const auto from_entity = core->getEntityPointer(from_entity_name)) {
193 if (
const auto to_entity = core->getEntityPointer(to_entity_name)) {
194 if (
const auto from_lanelet_pose = from_entity->getCanonicalizedLaneletPose()) {
195 if (
const auto to_lanelet_pose = to_entity->getCanonicalizedLaneletPose()) {
197 from_lanelet_pose.value(), from_entity->getBoundingBox(), to_lanelet_pose.value(),
198 to_entity->getBoundingBox(), routing_algorithm);
208 const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined)
210 if (
const auto from_entity = core->getEntityPointer(from_entity_name)) {
211 if (
const auto from_lanelet_pose = from_entity->getCanonicalizedLaneletPose()) {
213 from_lanelet_pose.value(), from_entity->getBoundingBox(), to_lanelet_pose,
225 const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined)
230 (routing_algorithm == RoutingAlgorithm::value_type::shortest);
232 from_lanelet_pose, from_bounding_box, to_lanelet_pose, to_bounding_box,
233 routing_configuration);
239 if (
const auto from_entity = core->getEntityPointer(from_entity_name)) {
240 if (
const auto to_entity = core->getEntityPointer(to_entity_name)) {
243 from_entity->getMapPose(), from_entity->getBoundingBox(), to_entity->getMapPose(),
244 to_entity->getBoundingBox())) {
245 return relative_pose.value();
255 if (
const auto from_entity = core->getEntityPointer(from_entity_name)) {
258 from_entity->getMapPose(), from_entity->getBoundingBox(), to_map_pose,
260 return relative_pose.value();
268 const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) ->
int
271 const auto from_lanelet_pose =
272 core->getEntity(from_entity_name).getCanonicalizedLaneletPose()) {
274 const auto to_lanelet_pose =
275 core->getEntity(to_entity_name).getCanonicalizedLaneletPose()) {
278 (routing_algorithm == RoutingAlgorithm::value_type::shortest);
281 from_lanelet_pose.value(), to_lanelet_pose.value(), routing_configuration,
282 core->getHdmapUtils())) {
283 return lane_changes.value().first - lane_changes.value().second;
288 "Failed to evaluate lateral relative lanes between ", from_entity_name,
" and ",
296 template <
typename... Ts>
299 return core->getEntity(entity_ref).requestAcquirePosition(std::forward<decltype(
xs)>(
xs)...);
302 template <
typename... Ts>
305 return core->spawn(std::forward<decltype(
xs)>(
xs)...);
308 template <
typename EntityRef,
typename DynamicConstra
ints>
312 auto & entity = core->getEntity(entity_ref);
313 entity.setBehaviorParameter([&]() {
314 auto behavior_parameter = entity.getBehaviorParameter();
317 const auto setIfNotInf = [](
auto & target,
const auto & value) {
318 if (not std::isinf(value)) {
322 setIfNotInf(behavior_parameter.dynamic_constraints.max_speed, dynamic_constraints.max_speed);
323 setIfNotInf(behavior_parameter.dynamic_constraints.max_acceleration, dynamic_constraints.max_acceleration);
324 setIfNotInf(behavior_parameter.dynamic_constraints.max_acceleration_rate, dynamic_constraints.max_acceleration_rate);
325 setIfNotInf(behavior_parameter.dynamic_constraints.max_deceleration, dynamic_constraints.max_deceleration);
326 setIfNotInf(behavior_parameter.dynamic_constraints.max_deceleration_rate, dynamic_constraints.max_deceleration_rate);
329 return behavior_parameter;
333 template <
typename Controller>
337 auto & entity = core->getEntity(entity_ref);
338 entity.setVelocityLimit(controller.properties.template get<Double>(
339 "maxSpeed", std::numeric_limits<Double::value_type>::max()));
341 if (controller.properties.contains(
"lateralCollisionThreshold")) {
342 entity.setLateralCollisionThreshold(
343 controller.properties.template get<Double>(
"lateralCollisionThreshold"));
345 entity.setLateralCollisionThreshold(std::nullopt);
347 entity.setBehaviorParameter([&]() {
348 auto message = entity.getBehaviorParameter();
349 message.see_around = not controller.properties.template get<Boolean>(
"isBlind");
351 message.dynamic_constraints.max_acceleration =
352 controller.properties.template get<Double>(
"maxAcceleration", 10.0);
353 message.dynamic_constraints.max_acceleration_rate =
354 controller.properties.template get<Double>(
"maxAccelerationRate", 3.0);
355 message.dynamic_constraints.max_deceleration =
356 controller.properties.template get<Double>(
"maxDeceleration", 10.0);
357 message.dynamic_constraints.max_deceleration_rate =
358 controller.properties.template get<Double>(
"maxDecelerationRate", 3.0);
359 message.dynamic_constraints.max_speed =
360 controller.properties.template get<Double>(
"maxSpeed", 50.0);
364 if (controller.isAutoware()) {
365 core->attachImuSensor(entity_ref, [&]() {
366 simulation_api_schema::ImuSensorConfiguration configuration;
367 configuration.set_entity(entity_ref);
368 configuration.set_frame_id(
"base_link");
369 configuration.set_add_gravity(
true);
370 configuration.set_use_seed(
true);
371 configuration.set_seed(0);
372 configuration.set_noise_standard_deviation_orientation(0.01);
373 configuration.set_noise_standard_deviation_twist(0.01);
374 configuration.set_noise_standard_deviation_acceleration(0.01);
375 return configuration;
378 core->attachLidarSensor([&]() {
379 simulation_api_schema::LidarConfiguration configuration;
381 auto degree_to_radian = [](
auto degree) {
382 return degree / 180.0 * boost::math::constants::pi<double>();
386 configuration.set_architecture_type(common::getParameter<std::string>(
"architecture_type",
"awf/universe/20240605"));
387 configuration.set_entity(entity_ref);
388 configuration.set_horizontal_resolution(degree_to_radian(controller.properties.template get<Double>(
"pointcloudHorizontalResolution", 1.0)));
389 configuration.set_lidar_sensor_delay(controller.properties.template get<Double>(
"pointcloudPublishingDelay"));
390 configuration.set_scan_duration(0.1);
393 const auto vertical_field_of_view = degree_to_radian(
394 controller.properties.template get<Double>(
"pointcloudVerticalFieldOfView", 30.0));
396 const auto channels =
397 controller.properties.template get<UnsignedInteger>(
"pointcloudChannels", 16);
399 for (std::size_t i = 0; i < channels; ++i) {
400 configuration.add_vertical_angles(
401 vertical_field_of_view / 2 - vertical_field_of_view / channels * i);
404 return configuration;
407 core->attachDetectionSensor([&]() {
408 simulation_api_schema::DetectionSensorConfiguration configuration;
410 configuration.set_architecture_type(common::getParameter<std::string>(
"architecture_type",
"awf/universe/20240605"));
411 configuration.set_entity(entity_ref);
412 configuration.set_detect_all_objects_in_range(controller.properties.template get<Boolean>(
"isClairvoyant"));
413 configuration.set_object_recognition_delay(controller.properties.template get<Double>(
"detectedObjectPublishingDelay"));
414 configuration.set_pos_noise_stddev(controller.properties.template get<Double>(
"detectedObjectPositionStandardDeviation"));
415 configuration.set_probability_of_lost(controller.properties.template get<Double>(
"detectedObjectMissingProbability"));
416 configuration.set_random_seed(controller.properties.template get<UnsignedInteger>(
"randomSeed"));
417 configuration.set_range(controller.properties.template get<Double>(
"detectionSensorRange",300.0));
418 configuration.set_object_recognition_ground_truth_delay(controller.properties.template get<Double>(
"detectedObjectGroundTruthPublishingDelay"));
419 configuration.set_update_duration(0.1);
421 return configuration;
424 core->attachOccupancyGridSensor([&]() {
425 simulation_api_schema::OccupancyGridSensorConfiguration configuration;
427 configuration.set_architecture_type(common::getParameter<std::string>(
"architecture_type",
"awf/universe/20240605"));
428 configuration.set_entity(entity_ref);
429 configuration.set_filter_by_range(controller.properties.template get<Boolean>(
"isClairvoyant"));
430 configuration.set_height(200);
431 configuration.set_range(300);
432 configuration.set_resolution(0.5);
433 configuration.set_update_duration(0.1);
434 configuration.set_width(200);
436 return configuration;
439 core->attachPseudoTrafficLightDetector([&]() {
440 simulation_api_schema::PseudoTrafficLightDetectorConfiguration configuration;
441 configuration.set_architecture_type(
442 common::getParameter<std::string>(
"architecture_type",
"awf/universe/20240605"));
443 return configuration;
446 auto & ego_entity = core->getEgoEntity(entity_ref);
448 if (controller.properties.contains(
"allowGoalModification")) {
449 ego_entity.setParameter<
bool>(
450 "allow_goal_modification",
451 controller.properties.template get<Boolean>(
"allowGoalModification"));
454 for (
const auto & module :
456 manual_modules_string.erase(
458 manual_modules_string.begin(), manual_modules_string.end(),
459 [](
const auto & c) { return std::isspace(c); }),
460 manual_modules_string.end());
462 std::vector<std::string> modules;
464 std::istringstream modules_stream(manual_modules_string);
465 while (std::getline(modules_stream, buffer,
',')) {
466 modules.push_back(buffer);
469 }(controller.properties.template get<String>(
470 "featureIdentifiersRequiringExternalPermissionForAutonomousDecisions"))) {
472 ego_entity.requestAutoModeForCooperation(module,
false);
473 }
catch (
const Error & error) {
475 "featureIdentifiersRequiringExternalPermissionForAutonomousDecisions is not "
476 "supported in this environment: ",
483 template <
typename... Ts>
486 return core->getEntity(entity_ref).requestAssignRoute(std::forward<decltype(
xs)>(
xs)...);
489 template <
typename... Ts>
492 return core->despawn(std::forward<decltype(
xs)>(
xs)...);
495 template <
typename... Ts>
498 return core->getEntity(entity_ref).requestFollowTrajectory(std::forward<decltype(
xs)>(
xs)...);
501 template <
typename... Ts>
504 return core->getEntity(entity_ref).requestLaneChange(std::forward<decltype(
xs)>(
xs)...);
507 template <
typename... Ts>
510 return core->getEntity(entity_ref).requestSpeedChange(std::forward<decltype(
xs)>(
xs)...);
514 typename PoseType,
typename... Ts,
515 typename = std::enable_if_t<
521 return core->getEntity(name).setStatus(pose, std::forward<decltype(
xs)>(
xs)...);
524 template <
typename... Ts>
528 return core->getEntity(name).setStatus(
529 core->getEntity(reference_entity_name).getMapPose(), std::forward<decltype(
xs)>(
xs)...);
532 template <
typename... Ts>
535 return core->getEntity(entity_ref).requestWalkStraight(std::forward<decltype(
xs)>(
xs)...);
545 return core->getEntity(name).getCurrentAccel().linear.x;
548 template <
typename... Ts>
551 return core->checkCollision(std::forward<decltype(
xs)>(
xs)...);
558 if (
const auto from_entity = core->getEntityPointer(from_entity_name)) {
559 if (
const auto to_entity = core->getEntityPointer(to_entity_name)) {
562 from_entity->getMapPose(), from_entity->getBoundingBox(), to_entity->getMapPose(),
563 to_entity->getBoundingBox())) {
568 return std::numeric_limits<double>::quiet_NaN();
573 if (
const auto observer = core->getEntityPointer(from.name())) {
574 if (
const auto observed = core->getEntityPointer(to.name())) {
575 auto velocity = [](
const auto & entity) -> Eigen::Vector3d {
576 auto direction = [](
const auto & q) -> Eigen::Vector3d {
577 return Eigen::Quaternion(q.w, q.x, q.y, q.z) * Eigen::Vector3d::UnitX();
579 return direction(entity->getMapPose().orientation) * entity->getCurrentTwist().linear.x;
582 const Eigen::Matrix3d rotation =
585 return rotation.transpose() * velocity(observed) -
586 rotation.transpose() * velocity(observer);
590 return Eigen::Vector3d(Double::nan(), Double::nan(), Double::nan());
593 template <
typename... Ts>
597 return core->getCurrentTime(std::forward<decltype(
xs)>(
xs)...);
599 return std::numeric_limits<double>::quiet_NaN();
613 const auto linear = core->getEntity(name).getCurrentTwist().linear;
614 return Eigen::Vector3d(linear.x, linear.y, linear.z);
619 return core->getEntity(name).getStandStillDuration();
625 if (
const auto from_entity = core->getEntityPointer(from_entity_name)) {
626 if (
const auto to_entity = core->getEntityPointer(to_entity_name)) {
628 from_entity->getMapPose(), to_entity->getMapPose());
629 relative_pose && relative_pose->position.x <= 0) {
630 const double time_headway =
631 (relative_pose->position.x * -1) / to_entity->getCurrentTwist().linear.x;
632 return std::isnan(time_headway) ? std::numeric_limits<double>::infinity()
637 return std::numeric_limits<double>::quiet_NaN();
644 template <
typename Performance,
typename Properties>
649 core->getEntity(entity_ref)
650 .activateOutOfRangeJob(
653 properties.template get<Double>(
"minJerk", Double::lowest()),
654 properties.template get<Double>(
"maxJerk", Double::max()));
659 return core->startNpcLogic();
662 template <
typename... Ts>
665 return core->getEntity(entity_ref).getCurrentAction(std::forward<decltype(
xs)>(
xs)...);
668 template <
typename EntityRef,
typename OSCLanePosition>
670 const EntityRef & entity_ref,
const OSCLanePosition & osc_lane_position)
672 if (
const auto entity = core->getEntityPointer(entity_ref)) {
673 const auto from_map_pose = entity->getMapPose();
676 const auto relative_pose =
678 return static_cast<Double>(std::abs(
682 return Double::nan();
685 template <
typename EntityRef>
688 if (
const auto entity = core->getEntityPointer(entity_ref)) {
689 if (
const auto canonicalized_lanelet_pose = entity->getCanonicalizedLaneletPose()) {
690 return static_cast<Double>(std::abs(
694 return Double::nan();
699 return core->getEgoEntity(ego_ref).engage();
704 return core->getEgoEntity(ego_ref).isEngageable();
709 return core->getEgoEntity(ego_ref).isEngaged();
712 template <
typename... Ts>
716 if (
const auto ego_name = core->getFirstEgoName()) {
717 return core->getEgoEntity(ego_name.value())
718 .sendCooperateCommand(std::forward<decltype(
xs)>(
xs)...);
726 return core->getEgoEntity(ego_ref).getMinimumRiskManeuverBehaviorName();
731 return core->getEgoEntity(ego_ref).getMinimumRiskManeuverStateName();
736 return core->getEgoEntity(ego_ref).getEmergencyStateName();
741 return core->getEgoEntity(ego_ref).getTurnIndicatorsCommandName();
745 template <
typename... Ts>
748 return core->getConventionalTrafficLights()->setTrafficLightsState(
749 std::forward<decltype(
xs)>(
xs)...);
752 template <
typename... Ts>
755 return core->getConventionalTrafficLights()->setTrafficLightsConfidence(
756 std::forward<decltype(
xs)>(
xs)...);
759 template <
typename... Ts>
762 return core->getConventionalTrafficLights()->getTrafficLightsComposedState(
763 std::forward<decltype(
xs)>(
xs)...);
766 template <
typename... Ts>
769 return core->getConventionalTrafficLights()->compareTrafficLightsState(
770 std::forward<decltype(
xs)>(
xs)...);
773 template <
typename... Ts>
776 return core->getConventionalTrafficLights()->resetUpdate(std::forward<decltype(
xs)>(
xs)...);
779 template <
typename... Ts>
782 return core->getV2ITrafficLights()->setTrafficLightsState(std::forward<decltype(
xs)>(
xs)...);
785 template <
typename... Ts>
788 return core->getV2ITrafficLights()->resetUpdate(std::forward<decltype(
xs)>(
xs)...);
Definition: simulator_core.hpp:294
static auto applyWalkStraightAction(const std::string &entity_ref, Ts &&... xs)
Definition: simulator_core.hpp:533
static auto applyFollowTrajectoryAction(const std::string &entity_ref, Ts &&... xs)
Definition: simulator_core.hpp:496
static auto applyAcquirePositionAction(const std::string &entity_ref, Ts &&... xs)
Definition: simulator_core.hpp:297
static auto applyAssignRouteAction(const std::string &entity_ref, Ts &&... xs)
Definition: simulator_core.hpp:484
static auto applyAssignControllerAction(const std::string &entity_ref, Controller &&controller) -> void
Definition: simulator_core.hpp:334
static auto applyLaneChangeAction(const std::string &entity_ref, Ts &&... xs)
Definition: simulator_core.hpp:502
static auto applyProfileAction(const EntityRef &entity_ref, const DynamicConstraints &dynamic_constraints) -> void
Definition: simulator_core.hpp:309
static auto applyDeleteEntityAction(Ts &&... xs)
Definition: simulator_core.hpp:490
static auto applyTeleportAction(const std::string &name, const PoseType &pose, Ts &&... xs)
Definition: simulator_core.hpp:519
static auto applySpeedAction(const std::string &entity_ref, Ts &&... xs)
Definition: simulator_core.hpp:508
static auto applyAddEntityAction(Ts &&... xs) -> decltype(auto)
Definition: simulator_core.hpp:303
static auto applyTeleportAction(const std::string &name, const std::string &reference_entity_name, Ts &&... xs)
Definition: simulator_core.hpp:525
Definition: simulator_core.hpp:541
static auto evaluateStandStill(const std::string &name)
Definition: simulator_core.hpp:617
static auto evaluateCollisionCondition(Ts &&... xs) -> bool
Definition: simulator_core.hpp:549
static auto evaluateAcceleration(const std::string &name)
Definition: simulator_core.hpp:543
static auto evaluateSimulationTime(Ts &&... xs) -> double
Definition: simulator_core.hpp:594
static auto evaluateBoundingBoxEuclideanDistance(const std::string &from_entity_name, const std::string &to_entity_name)
Definition: simulator_core.hpp:554
static auto evaluateRelativeSpeed(const Entity &from, const Entity &to) -> Eigen::Vector3d
Definition: simulator_core.hpp:571
static auto evaluateSpeed(const std::string &name)
Definition: simulator_core.hpp:603
static auto evaluateTimeHeadway(const std::string &from_entity_name, const std::string &to_entity_name)
Definition: simulator_core.hpp:622
Definition: simulator_core.hpp:72
static auto makeNativeRelativeWorldPosition(const std::string &from_entity_name, const std::string &to_entity_name)
Definition: simulator_core.hpp:108
static auto makeNativeBoundingBoxRelativeWorldPosition(const std::string &from_entity_name, const std::string &to_entity_name)
Definition: simulator_core.hpp:236
static auto makeNativeRelativeLanePosition(const std::string &from_entity_name, const NativeLanePosition &to_lanelet_pose, const RoutingAlgorithm::value_type routing_algorithm=RoutingAlgorithm::undefined) -> traffic_simulator::LaneletPose
Definition: simulator_core.hpp:162
static auto makeNativeBoundingBoxRelativeLanePosition(const std::string &from_entity_name, const std::string &to_entity_name, const RoutingAlgorithm::value_type routing_algorithm=RoutingAlgorithm::undefined)
Definition: simulator_core.hpp:188
static auto makeNativeBoundingBoxRelativeLanePosition(const NativeLanePosition &from_lanelet_pose, const traffic_simulator_msgs::msg::BoundingBox &from_bounding_box, const NativeLanePosition &to_lanelet_pose, const traffic_simulator_msgs::msg::BoundingBox &to_bounding_box, const RoutingAlgorithm::value_type routing_algorithm=RoutingAlgorithm::undefined) -> traffic_simulator::LaneletPose
Definition: simulator_core.hpp:220
static auto convert(const NativeLanePosition &native_lane_position) -> NativeWorldPosition
Definition: simulator_core.hpp:103
static auto convert(const NativeWorldPosition &pose) -> NativeLanePosition
Definition: simulator_core.hpp:81
static auto evaluateLateralRelativeLanes(const std::string &from_entity_name, const std::string &to_entity_name, const RoutingAlgorithm::value_type routing_algorithm=RoutingAlgorithm::undefined) -> int
Definition: simulator_core.hpp:266
static auto makeNativeRelativeLanePosition(const NativeLanePosition &from_lanelet_pose, const NativeLanePosition &to_lanelet_pose, const RoutingAlgorithm::value_type routing_algorithm=RoutingAlgorithm::undefined) -> traffic_simulator::LaneletPose
Definition: simulator_core.hpp:176
static auto makeNativeBoundingBoxRelativeWorldPosition(const std::string &from_entity_name, const NativeWorldPosition &to_map_pose)
Definition: simulator_core.hpp:252
static auto makeNativeRelativeLanePosition(const std::string &from_entity_name, const std::string &to_entity_name, const RoutingAlgorithm::value_type routing_algorithm=RoutingAlgorithm::undefined) -> traffic_simulator::LaneletPose
Definition: simulator_core.hpp:148
static auto canonicalize(const traffic_simulator::LaneletPose &non_canonicalized) -> NativeLanePosition
Definition: simulator_core.hpp:74
static auto makeNativeRelativeWorldPosition(const std::string &from_entity_name, const NativeWorldPosition &to_map_pose)
Definition: simulator_core.hpp:122
static auto makeNativeRelativeWorldPosition(const NativeWorldPosition &from_map_pose, const std::string &to_entity_name)
Definition: simulator_core.hpp:135
static auto makeNativeBoundingBoxRelativeLanePosition(const std::string &from_entity_name, const NativeLanePosition &to_lanelet_pose, const RoutingAlgorithm::value_type routing_algorithm=RoutingAlgorithm::undefined)
Definition: simulator_core.hpp:206
Definition: simulator_core.hpp:642
static auto isEngageable(const std::string &ego_ref) -> decltype(auto)
Definition: simulator_core.hpp:702
static auto evaluateRelativeHeading(const EntityRef &entity_ref)
Definition: simulator_core.hpp:686
static auto getMinimumRiskManeuverStateName(const std::string &ego_ref) -> decltype(auto)
Definition: simulator_core.hpp:729
static auto setV2ITrafficLightsState(Ts &&... xs) -> decltype(auto)
Definition: simulator_core.hpp:780
static auto resetV2ITrafficLightPublishRate(Ts &&... xs) -> decltype(auto)
Definition: simulator_core.hpp:786
static auto setConventionalTrafficLightsState(Ts &&... xs) -> decltype(auto)
Definition: simulator_core.hpp:746
static auto compareConventionalTrafficLightsState(Ts &&... xs) -> decltype(auto)
Definition: simulator_core.hpp:767
static auto resetConventionalTrafficLightPublishRate(Ts &&... xs) -> decltype(auto)
Definition: simulator_core.hpp:774
static auto getMinimumRiskManeuverBehaviorName(const std::string &ego_ref) -> decltype(auto)
Definition: simulator_core.hpp:724
static auto evaluateRelativeHeading(const EntityRef &entity_ref, const OSCLanePosition &osc_lane_position)
Definition: simulator_core.hpp:669
static auto getConventionalTrafficLightsComposedState(Ts &&... xs) -> decltype(auto)
Definition: simulator_core.hpp:760
static auto getTurnIndicatorsCommandName(const std::string &ego_ref) -> decltype(auto)
Definition: simulator_core.hpp:739
static auto sendCooperateCommand(Ts &&... xs) -> decltype(auto)
Definition: simulator_core.hpp:713
static auto activatePerformanceAssertion(const std::string &entity_ref, const Performance &performance, const Properties &properties)
Definition: simulator_core.hpp:645
static auto engage(const std::string &ego_ref) -> decltype(auto)
Definition: simulator_core.hpp:697
static auto evaluateCurrentState(const std::string &entity_ref, Ts &&... xs) -> decltype(auto)
Definition: simulator_core.hpp:663
static auto setConventionalTrafficLightConfidence(Ts &&... xs) -> decltype(auto)
Definition: simulator_core.hpp:753
static auto activateNonUserDefinedControllers() -> decltype(auto)
Definition: simulator_core.hpp:657
static auto isEngaged(const std::string &ego_ref) -> decltype(auto)
Definition: simulator_core.hpp:707
static auto getEmergencyStateName(const std::string &ego_ref) -> decltype(auto)
Definition: simulator_core.hpp:734
Definition: simulator_core.hpp:42
static auto deactivate() -> void
Definition: simulator_core.hpp:60
static auto active()
Definition: simulator_core.hpp:58
static auto activate(const Node &node, const traffic_simulator::Configuration &configuration, Ts &&... xs) -> void
Definition: simulator_core.hpp:47
static auto update() -> void
Definition: simulator_core.hpp:69
Definition: lanelet_pose.hpp:28
auto getRotationMatrix(T quat) -> Eigen::Matrix3d
Definition: get_rotation_matrix.hpp:33
auto convertQuaternionToEulerAngle(const T &q)
Definition: quaternion_to_euler.hpp:30
traffic_simulator::CanonicalizedLaneletPose NativeLanePosition
Definition: simulator_core.hpp:37
geometry_msgs::msg::Pose NativeWorldPosition
Definition: simulator_core.hpp:33
traffic_simulator::LaneletPose NativeRelativeLanePosition
Definition: simulator_core.hpp:39
NativeWorldPosition NativeRelativeWorldPosition
Definition: simulator_core.hpp:35
auto distance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2)
Definition: detection_sensor.cpp:43
auto countLaneChanges(const CanonicalizedLaneletPose &from, const CanonicalizedLaneletPose &to, const traffic_simulator::RoutingConfiguration &routing_configuration, const std::shared_ptr< hdmap_utils::HdMapUtils > &hdmap_utils_ptr) -> std::optional< std::pair< int, int >>
Definition: distance.cpp:51
auto boundingBoxDistance(const geometry_msgs::msg::Pose &from, const traffic_simulator_msgs::msg::BoundingBox &from_bounding_box, const geometry_msgs::msg::Pose &to, const traffic_simulator_msgs::msg::BoundingBox &to_bounding_box) -> std::optional< double >
Definition: distance.cpp:134
geometry_msgs::msg::Pose Pose
Definition: lanelet_wrapper.hpp:66
traffic_simulator_msgs::msg::BoundingBox BoundingBox
Definition: lanelet_wrapper.hpp:62
auto relativeLaneletPose(const CanonicalizedLaneletPose &from, const CanonicalizedLaneletPose &to, const RoutingConfiguration &routing_configuration) -> LaneletPose
Definition: pose.cpp:285
auto quietNaNPose() -> geometry_msgs::msg::Pose
Definition: pose.cpp:34
auto quietNaNLaneletPose() -> LaneletPose
Definition: pose.cpp:44
auto boundingBoxRelativeLaneletPose(const CanonicalizedLaneletPose &from, const traffic_simulator_msgs::msg::BoundingBox &from_bounding_box, const CanonicalizedLaneletPose &to, const traffic_simulator_msgs::msg::BoundingBox &to_bounding_box, const RoutingConfiguration &routing_configuration) -> LaneletPose
Definition: pose.cpp:314
auto boundingBoxRelativePose(const geometry_msgs::msg::Pose &from, const traffic_simulator_msgs::msg::BoundingBox &from_bounding_box, const geometry_msgs::msg::Pose &to, const traffic_simulator_msgs::msg::BoundingBox &to_bounding_box) -> std::optional< geometry_msgs::msg::Pose >
Definition: pose.cpp:263
auto relativePose(const geometry_msgs::msg::Pose &from, const geometry_msgs::msg::Pose &to) -> std::optional< geometry_msgs::msg::Pose >
Definition: pose.cpp:241
auto toCanonicalizedLaneletPose(const LaneletPose &lanelet_pose) -> std::optional< CanonicalizedLaneletPose >
Definition: pose.cpp:104
auto toMapPose(const CanonicalizedLaneletPose &lanelet_pose) -> geometry_msgs::msg::Pose
Definition: pose.cpp:87
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:23
Definition: junit5.hpp:25
std::string string
Definition: junit5.hpp:26
Definition: exception.hpp:27
Definition: controller.hpp:46
Definition: double.hpp:25
Definition: dynamic_constraints.hpp:44
Definition: entity_ref.hpp:35
Definition: entity.hpp:46
Definition: properties.hpp:42
Definition: configuration.hpp:31
Definition: routing_configuration.hpp:24
bool allow_lane_change
Definition: routing_configuration.hpp:29
traffic_simulator::lanelet_pose::CanonicalizedLaneletPose CanonicalizedLaneletPose
Definition: test_lanelet_pose.cpp:21