Merge pull request tier4#1325 from tier4/feature/interpreter/lidar-configuration

Feature/interpreter/lidar configuration

Author: hakuturu583

docs/developer_guide/ConfiguringPerceptionTopics.md

@@ -24,13 +24,16 @@ where `<NAME>` and `<VALUE>` can be set to:
 
 | Name                                       | Value                                         | Default | Description                                                                                                                                                                                                           |
 |--------------------------------------------|-----------------------------------------------|:-------:|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
-| `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.                                                                                                                                    |
-| `detectedObjectGroundTruthPublishingDelay` | A positive `double` type value                |  `0.0`  | Delays the publication of the perception ground truth topic by the specified number of seconds.                                                                                                                       |
+| `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.                                                                                                                                    |
+| `detectedObjectGroundTruthPublishingDelay` | A positive `double` type value                | `0.0`   | Delays the publication of the perception ground truth topic by the specified number of seconds.                                                                                                                       |
 | `detectionSensorRange`                     | A positive `double` type value                | `300.0` | Specifies the sensor detection range for detected object.                                                                                                                                                             |
 | `isClairvoyant`                            | A `boolean` type value                        | `false` | Specifies whether the detected object is a Clairvoyant. If this parameter is not defined explicitly, the property of `detectionSensorRange` is not reflected and only detected object detected by lidar is published. |
-| `randomSeed`                               | A positive `integer` type value               |   `0`   | Specifies the seed value for the random number generator.                                                                                                                                                             |
+| `pointcloudChannels`                       | A positive `integer` type value               | `16`    | Number of channels of pseudo LiDAR inside the simulator used to generate pointclouds.                                                                                                                                 |
+| `pointcloudHorizontalResolution`           | A positive `double` type value                | `1.0`   | Horizontal angular resolution of the pseudo LiDAR inside the simulator used to generate the pointcloud.                                                                                                               |
+| `pointcloudVerticalFieldOfView`            | A positive `double` type value                | `30.0`  | Vertical field of view of the pseudo LiDAR inside the simulator used to generate the pointcloud.                                                                                                                      |
+| `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
 specified at the same time. However, these properties only take effect for
@@ -161,7 +164,7 @@ 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
+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.
 
@@ -230,6 +233,114 @@ it reaches the receiver node.
                   value: "3"
 ```
 
+## Property `pointcloudChannels`
+
+**Summary** - Number of channels of pseudo LiDAR inside the simulator used to
+generate pointclouds.
+
+**Purpose** - The `simple_sensor_simulator` simulates a simple LiDAR, such as a
+horizontally rotating vertically aligned laser, typical of the VLP-16, to
+generate a pointcloud. The default settings produce a pointcloud that exactly
+mimics the sensing results from the VLP-16. However, VLP-16 is a relatively low
+pointcloud density LiDAR used with Autoware, so if a higher pointcloud density
+LiDAR is to be installed, it will be necessary to simulate a scenario with a
+higher pointcloud density to match the actual vehicle. This property addresses
+this issue by providing a means to specify the number of pseudo LiDAR channels.
+
+**Specification** - The property value must be a real number greater than or
+equal to 1. Zero or negative values are errors. The upper limit of values is
+the maximum value of a 64-bit unsigned integer, but computer performance
+effectively limits the value to much lower values.
+
+**Guarantee** - The `simple_sensor_simulator` does not simulate a realistic
+LiDAR. For example, in the case of a LiDAR with a mechanically rotating laser
+structure, the resulting point cloud will be distorted when moving at high
+speeds, but `simple_sensor_simulator` cannot simulate such behavior and
+produces an undistorted pointcloud.
+
+**Default behavior** - If the property is not specified, the default value is
+`"16"`. When the properties `pointcloudChannels` and
+`pointcloudVerticalFieldOfView` are both at their default values, the behavior
+mimics Velodyne VLP-16.
+
+**[Example](https://github.com/tier4/scenario_simulator_v2/blob/487556437b448186e2de484f5130eb2b1d015e74/test_runner/scenario_test_runner/scenario/Property.pointcloudVerticalFieldOfView.yaml)** -
+```
+        ObjectController:
+          Controller:
+            name: '...'
+            Properties:
+              Property:
+                - name: 'isEgo'
+                  value: 'true'
+                - name: 'pointcloudChannels'
+                  value: '128'
+```
+
+## Property `pointcloudHorizontalResolution`
+
+**Summary** - Horizontal angular resolution of the pseudo LiDAR inside the
+simulator used to generate the pointcloud.
+
+**Purpose** - To address the same issues as the property `pointcloudChannels`,
+this property provides a means to specify the angular resolution of the pseudo
+LiDAR.
+
+**Specification** - The property value must be a real number greater than zero.
+The unit is degrees. If the value is zero or negative, it is an error.
+
+**Guarantee** - Same as one for `pointcloudChannels`
+
+**Default behavior** - If the property is not specified, the default value is
+`"1.0"`.
+
+**[Example](https://github.com/tier4/scenario_simulator_v2/blob/487556437b448186e2de484f5130eb2b1d015e74/test_runner/scenario_test_runner/scenario/Property.pointcloudVerticalFieldOfView.yaml)** -
+```
+        ObjectController:
+          Controller:
+            name: '...'
+            Properties:
+              Property:
+                - name: 'isEgo'
+                  value: 'true'
+                - name: 'pointcloudHorizontalResolution'
+                  value: '1.5'
+```
+
+## Property `pointcloudVerticalFieldOfView`
+
+**Summary** - Vertical field of view of the pseudo LiDAR inside the simulator
+used to generate the pointcloud.
+
+**Purpose** - To address the same issues as the property `pointcloudChannels`,
+this property provides a means to specify the vertical field of view of the
+pseudo LiDAR.
+
+**Specification** - The property value must be a real number greater than zero.
+The unit is degrees. A value of zero or negative is an error. The specified
+angle is assigned equally up and down to the horizontal plane as viewed from
+the pseudo LiDAR. For example, if the value `30.0` is specified, the vertical
+field of view is +15° to -15°.
+
+**Guarantee** - Same as one for `pointcloudChannels`
+
+**Default behavior** - If the property is not specified, the default value is
+`"30.0"`. When the properties `pointcloudChannels` and
+`pointcloudVerticalFieldOfView` are both at their default values, the behavior
+mimics Velodyne VLP-16.
+
+**[Example](https://github.com/tier4/scenario_simulator_v2/blob/487556437b448186e2de484f5130eb2b1d015e74/test_runner/scenario_test_runner/scenario/Property.pointcloudVerticalFieldOfView.yaml)** -
+```
+        ObjectController:
+          Controller:
+            name: '...'
+            Properties:
+              Property:
+                - name: 'isEgo'
+                  value: 'true'
+                - name: 'pointcloudVerticalFieldOfView'
+                  value: '45.678'
+```
+
 ## Property `randomSeed`
 
 **Summary** - Specifies the seed value for the random number generator.

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -334,8 +334,34 @@ class SimulatorCore
       }());
 
       if (controller.isAutoware()) {
-        core->attachLidarSensor(
-          entity_ref, controller.properties.template get<Double>("pointcloudPublishingDelay"));
+        core->attachLidarSensor([&]() {
+          simulation_api_schema::LidarConfiguration configuration;
+
+          auto degree_to_radian = [](auto degree) {
+            return degree / 180.0 * boost::math::constants::pi<double>();
+          };
+
+          // clang-format off
+          configuration.set_architecture_type(core->getROS2Parameter<std::string>("architecture_type", "awf/universe"));
+          configuration.set_entity(entity_ref);
+          configuration.set_horizontal_resolution(degree_to_radian(controller.properties.template get<Double>("pointcloudHorizontalResolution", 1.0)));
+          configuration.set_lidar_sensor_delay(controller.properties.template get<Double>("pointcloudPublishingDelay"));
+          configuration.set_scan_duration(0.1);
+          // clang-format on
+
+          const auto vertical_field_of_view = degree_to_radian(
+            controller.properties.template get<Double>("pointcloudVerticalFieldOfView", 30.0));
+
+          const auto channels =
+            controller.properties.template get<UnsignedInteger>("pointcloudChannels", 16);
+
+          for (std::size_t i = 0; i < channels; ++i) {
+            configuration.add_vertical_angles(
+              vertical_field_of_view / 2 - vertical_field_of_view / channels * i);
+          }
+
+          return configuration;
+        }());
 
         core->attachDetectionSensor([&]() {
           simulation_api_schema::DetectionSensorConfiguration configuration;

simulation/traffic_simulator/src/visualization/visualization_component.cpp

@@ -124,18 +124,20 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
   constexpr auto default_quaternion = rosidl_runtime_cpp::MessageInitialization::DEFAULTS_ONLY;
   auto ret = visualization_msgs::msg::MarkerArray();
   auto stamp = get_clock()->now();
-  std_msgs::msg::ColorRGBA color;
-  switch (status.type.type) {
-    case status.type.EGO:
-      color = color_names::makeColorMsg("limegreen", 0.99);
-      break;
-    case status.type.PEDESTRIAN:
-      color = color_names::makeColorMsg("orange", 0.99);
-      break;
-    case status.type.VEHICLE:
-      color = color_names::makeColorMsg("lightskyblue", 0.99);
-      break;
-  }
+
+  const auto color = [&]() {
+    switch (status.type.type) {
+      case status.type.EGO:
+        return color_names::makeColorMsg("limegreen", 0.99);
+      case status.type.PEDESTRIAN:
+        return color_names::makeColorMsg("orange", 0.99);
+      case status.type.VEHICLE:
+        return color_names::makeColorMsg("lightskyblue", 0.99);
+      default:
+      case status.type.MISC_OBJECT:
+        return color_names::makeColorMsg("magenta", 0.99);
+    }
+  }();
 
   if (goal_pose.size() != 0) {
     goal_pose_max_size = std::max(goal_pose_max_size, int(goal_pose.size()));