diff --git a/docs/developer_guide/ConfiguringPerceptionTopics.md b/docs/developer_guide/ConfiguringPerceptionTopics.md index 929551d1d13..eb9cc6c1680 100644 --- a/docs/developer_guide/ConfiguringPerceptionTopics.md +++ b/docs/developer_guide/ConfiguringPerceptionTopics.md @@ -27,6 +27,7 @@ where `` and `` can be set to: | `detectedObjectMissingProbability` | A `double` type value between `0.0` and `1.0` | `0.0` | Do not publish the perception topic with the given probability. | | `detectedObjectPositionStandardDeviation` | A positive `double` type value | `0.0` | Randomize the positions of other vehicles included in the perception topic according to the given standard deviation. | | `detectedObjectPublishingDelay` | A positive `double` type value | `0.0` | Delays the publication of the perception topic by the specified number of seconds. | +| `pointcloudPublishingDelay` | A positive `double` type value | `0.0` | Delays the publication of the no_ground pointcloud by the specified number of seconds. | | `randomSeed` | A positive `integer` type value | `0` | Specifies the seed value for the random number generator. | These properties are not exclusive. In other words, multiple properties can be @@ -128,6 +129,39 @@ not supported. `"0.0"`, meaning no randomization. **[Example](https://github.com/tier4/scenario_simulator_v2/blob/master/test_runner/scenario_test_runner/scenario/Property.detectedObjectPositionStandardDeviation.yaml)** - + +## Property `*PublishingDelay` +**Summary** - Delays the publication of the perception topic by the specified +number of seconds. There are two perception topic delay properties. + +- `detectedObjectPublishingDelay` delays the publication of detected object. +- `pointcloudPublishingDelay` delays the publication of no_ground pointcloud. + +**Purpose** - In a real vehicle, the perception topic is generated through the +processing of a sensing and perception module. However, in `scenario_simulator_v2`, +these processes are simplified. Detected objects are directly outputted as they +are from the simulator's objects, and no_ground pointcloud is the result of a +simple ray tracing. While this is suitable for testing planning and control modules +in an idealized environment, it does not account for the processing delays in +perception in a real vehicle. These properties can introduce an arbitrary delay +in the perception topic to simulate these real-world delays. + +**Specification** - For each frame, if the current stamp exceeds the set time +from the topic's stamp, the topic is published, thereby implementing the delay. +This could result in a maximum error of 2/FPS. Also, the order of topics to be +published is guaranteed to be the same as when there is no delay. +Since the delay is set to the same value for each object, it is not possible to +delay only a specific object. + +**Note** - This feature sets the delay from when the perception results are +obtained in `scenario_simulator_v2` to when they are published. Note that there +are other potential sources of delay, such as frame rates becoming unstable, +and delays in pub/sub. + +**Default behavior** - If the property is not defined, the default value is +`"0.0"`, indicating no delay. + +**[Example of detectedObjectPublishingDelay](https://github.com/tier4/scenario_simulator_v2/blob/master/test_runner/scenario_test_runner/scenario/Property.detectedObjectPositionStandardDeviation.yaml)** - ``` ObjectController: Controller: @@ -140,44 +174,7 @@ not supported. value: "3" ``` -## Property `detectedObjectPublishingDelay` - -**Summary** - Delays the publication of the perception topic by the specified -number of seconds. - -**Purpose** - Normally, Autoware reflects the surrounding situation in the -steering operation by processing the data in the order of the sensing module, -perception module, planning module, and vehicle driver. However, when not -connected with AWSIM, `scenario_simulator_v2` skips the sensing module and -perception module and directly generates the data of the perception result, and -sends it to the planning module. This behavior is desirable as a test of the -planning module, but on the other hand, there is a problem that the time until -the perception result is generated is unrealistically fast in response to -changes in the environment surrounding the vehicle. This property works around -this problem by setting an interval of the specified number of seconds between -`scenario_simulator_v2` generating a perception result and publishing it. - -**Specification** - The property's value must be a positive real number. The -unit is seconds. It is an error if the value is negative.　Since the delay is -set to the same value for each topic, it is not possible to delay only a -specific topic. - -**Guarantee** - This delay setting ensures that `scenario_simulator_v2` -publishes the perception results in a consistent order. They are published -according to their original order. However, while `scenario_simulator_v2` -guarantees to publish in order, it does not guarantee that it reaches the -planning module in order. This is because the arrival order of topics in ROS 2 -is not guaranteed. - -**Note** - This feature only adjusts the interval between ssv2 generating a -perception result and publishing it. Note that there is another kind of delay -between when `scenario_simulator_v2` publishes the perception result and when -it reaches the planning module. - -**Default behavior** - If the property is not specified, the default value is -`"0.0"`, meaning no delay. - -**[Example](https://github.com/tier4/scenario_simulator_v2/blob/master/test_runner/scenario_test_runner/scenario/Property.detectedObjectPublishingDelay.yaml)** - +**[Example of pointcloudPublishingDelay](https://github.com/tier4/scenario_simulator_v2/blob/master/test_runner/scenario_test_runner/scenario/Property.detectedObjectPublishingDelay.yaml)** - ``` ObjectController: Controller: diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 69691fcd20b..7ce90d5eca0 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -245,7 +245,8 @@ class SimulatorCore }()); if (controller.isUserDefinedController()) { - core->attachLidarSensor(entity_ref); + core->attachLidarSensor( + entity_ref, controller.properties.template get("pointcloudPublishingDelay")); core->attachDetectionSensor([&]() { simulation_api_schema::DetectionSensorConfiguration configuration; diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp index cdf6357cd0a..9b03a6e80c6 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -56,6 +57,8 @@ class LidarSensor : public LidarSensorBase { const typename rclcpp::Publisher::SharedPtr publisher_ptr_; + std::queue> queue_pointcloud_; + auto raycast(const std::vector &, const rclcpp::Time &) -> T; @@ -74,10 +77,18 @@ class LidarSensor : public LidarSensorBase { if (current_time - last_update_stamp_ - configuration_.scan_duration() >= -0.002) { last_update_stamp_ = current_time; - publisher_ptr_->publish(raycast(status, stamp)); + queue_pointcloud_.push(std::make_pair(raycast(status, stamp), current_time)); } else { detected_objects_ = {}; } + + if ( + !queue_pointcloud_.empty() && + current_time - queue_pointcloud_.front().second >= configuration_.lidar_sensor_delay()) { + const auto pointcloud = queue_pointcloud_.front().first; + queue_pointcloud_.pop(); + publisher_ptr_->publish(pointcloud); + } } private: diff --git a/simulation/simulation_interface/proto/simulation_api_schema.proto b/simulation/simulation_interface/proto/simulation_api_schema.proto index 5c4c1d3e635..1c1cb46e096 100644 --- a/simulation/simulation_interface/proto/simulation_api_schema.proto +++ b/simulation/simulation_interface/proto/simulation_api_schema.proto @@ -35,6 +35,7 @@ message LidarConfiguration { repeated double vertical_angles = 3; // Vertical resolutions of the lidar. (unit : radian) double scan_duration = 4; // Scan duration of the lidar. (unit: second) string architecture_type = 5; // Autoware architecture type. + double lidar_sensor_delay = 6; // lidar sensor delay. (unit : second) It delays publishing timing. } /** diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index d69e53db479..9420b2a231e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -236,7 +236,8 @@ class API bool attachLidarSensor(const simulation_api_schema::LidarConfiguration &); bool attachLidarSensor( - const std::string &, const helper::LidarType = traffic_simulator::helper::LidarType::VLP16); + const std::string &, const double lidar_sensor_delay, + const helper::LidarType = traffic_simulator::helper::LidarType::VLP16); bool attachDetectionSensor(const simulation_api_schema::DetectionSensorConfiguration &); bool attachDetectionSensor( diff --git a/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp b/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp index 04456004808..2c54aa934ff 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp @@ -116,7 +116,7 @@ enum class LidarType { VLP16, VLP32 }; const simulation_api_schema::LidarConfiguration constructLidarConfiguration( const LidarType type, const std::string & entity, const std::string & architecture_type, - const double horizontal_resolution = 1.0 / 180.0 * M_PI); + const double lidar_sensor_delay = 0, const double horizontal_resolution = 1.0 / 180.0 * M_PI); const simulation_api_schema::DetectionSensorConfiguration constructDetectionSensorConfiguration( const std::string & entity, const std::string & architecture_type, const double update_duration, diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index c0502071db9..f4c9c7ecf44 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -241,10 +241,13 @@ bool API::attachLidarSensor(const simulation_api_schema::LidarConfiguration & li } } -bool API::attachLidarSensor(const std::string & entity_name, const helper::LidarType lidar_type) +bool API::attachLidarSensor( + const std::string & entity_name, const double lidar_sensor_delay, + const helper::LidarType lidar_type) { return attachLidarSensor(helper::constructLidarConfiguration( - lidar_type, entity_name, getParameter("architecture_type", "awf/universe"))); + lidar_type, entity_name, getParameter("architecture_type", "awf/universe"), + lidar_sensor_delay)); } bool API::updateTrafficLightsInSim() diff --git a/simulation/traffic_simulator/src/helper/helper.cpp b/simulation/traffic_simulator/src/helper/helper.cpp index 7b7e347051f..49b2db7eee4 100644 --- a/simulation/traffic_simulator/src/helper/helper.cpp +++ b/simulation/traffic_simulator/src/helper/helper.cpp @@ -95,12 +95,13 @@ const simulation_api_schema::DetectionSensorConfiguration constructDetectionSens const simulation_api_schema::LidarConfiguration constructLidarConfiguration( const LidarType type, const std::string & entity, const std::string & architecture_type, - const double horizontal_resolution) + const double lidar_sensor_delay, const double horizontal_resolution) { simulation_api_schema::LidarConfiguration configuration; configuration.set_horizontal_resolution(horizontal_resolution); configuration.set_architecture_type(architecture_type); configuration.set_entity(entity); + configuration.set_lidar_sensor_delay(lidar_sensor_delay); switch (type) { case LidarType::VLP16: configuration.set_scan_duration(0.1); diff --git a/test_runner/scenario_test_runner/scenario/Property.pointcloudPublishingDelay.yaml b/test_runner/scenario_test_runner/scenario/Property.pointcloudPublishingDelay.yaml new file mode 100644 index 00000000000..2a4f2b38a46 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/Property.pointcloudPublishingDelay.yaml @@ -0,0 +1,239 @@ +ScenarioModifiers: + ScenarioModifier: [] +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 1 + date: "2023-06-29T03:59:44.001Z" + description: "" + author: Kosuke Takeuchi + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + CatalogLocation: [] + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map + TrafficSignals: + TrafficSignalController: [] + Entities: + ScenarioObject: + - name: ego + Vehicle: + name: "" + vehicleCategory: car + BoundingBox: + Center: + x: 1.355 + y: 0 + z: 1.25 + Dimensions: + width: 1.83 + length: 4.77 + height: 2.5 + Performance: + maxSpeed: 50 + maxAcceleration: INF + maxDeceleration: INF + Axles: + FrontAxle: + maxSteering: 0.5236 + wheelDiameter: 0.78 + trackWidth: 1.63 + positionX: 1.385 + positionZ: 0.39 + RearAxle: + maxSteering: 0.5236 + wheelDiameter: 0.78 + trackWidth: 1.63 + positionX: -1.355 + positionZ: 0.39 + Properties: + Property: [] + ObjectController: + Controller: + name: "" + Properties: + Property: + - name: "isEgo" + value: "true" + - name: "pointcloudPublishingDelay" + value: "3" + - name: Npc1 + Vehicle: + name: "" + vehicleCategory: car + BoundingBox: + Center: + x: 0 + y: 0 + z: 1.25 + Dimensions: + width: 1.8 + length: 4 + height: 2.5 + Performance: + maxSpeed: 50 + maxAcceleration: INF + maxDeceleration: INF + Axles: + FrontAxle: + maxSteering: 0.5236 + wheelDiameter: 0.6 + trackWidth: 1.8 + positionX: 2 + positionZ: 0.3 + RearAxle: + maxSteering: 0.5236 + wheelDiameter: 0.6 + trackWidth: 1.8 + positionX: 0 + positionZ: 0.3 + Properties: + Property: [] + ObjectController: + Controller: + name: "" + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: "" + laneId: "34513" + s: 1 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - RoutingAction: + AcquirePositionAction: + Position: + LanePosition: + roadId: "" + laneId: "34606" + s: 20 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - entityRef: Npc1 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: "" + laneId: "34513" + s: 30 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - ControllerAction: + AssignControllerAction: + Controller: + name: "" + Properties: + Property: + - name: maxSpeed + value: "5" + Story: + - name: "" + Act: + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: "" + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: "" + Event: + - name: "" + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: "" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: + LanePosition: + roadId: "" + laneId: "34606" + s: 20 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + tolerance: 1 + Action: + - name: "" + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: "" + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: "" + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 180 + rule: greaterThan + - Condition: + - name: "" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + CollisionCondition: + EntityRef: + entityRef: Npc1 + Action: + - name: "" + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: "" + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: []