refactor(simple_planning_simulator): rework parameters

simulator/simple_planning_simulator/CMakeLists.txt

@@ -43,4 +43,9 @@ if(BUILD_TESTING)
   )
 endif()
 
-ament_auto_package(INSTALL_TO_SHARE param data launch test)
+ament_auto_package(INSTALL_TO_SHARE
+    config
+    data
+    launch
+    test
+)

simulator/simple_planning_simulator/README.md

@@ -40,17 +40,7 @@ The purpose of this simulator is for the integration test of planning and contro
 
 ### Common Parameters
 
-| Name                  | Type   | Description                                                                                                                                | Default value        |
-| :-------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------------- |
-| simulated_frame_id    | string | set to the child_frame_id in output tf                                                                                                     | "base_link"          |
-| origin_frame_id       | string | set to the frame_id in output tf                                                                                                           | "odom"               |
-| initialize_source     | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `input/initialpose` topic is published. | "INITIAL_POSE_TOPIC" |
-| add_measurement_noise | bool   | If true, the Gaussian noise is added to the simulated results.                                                                             | true                 |
-| pos_noise_stddev      | double | Standard deviation for position noise                                                                                                      | 0.01                 |
-| rpy_noise_stddev      | double | Standard deviation for Euler angle noise                                                                                                   | 0.0001               |
-| vel_noise_stddev      | double | Standard deviation for longitudinal velocity noise                                                                                         | 0.0                  |
-| angvel_noise_stddev   | double | Standard deviation for angular velocity noise                                                                                              | 0.0                  |
-| steer_noise_stddev    | double | Standard deviation for steering angle noise                                                                                                | 0.0001               |
+{{ json_to_markdown("simulator/simple_planning_simulator/schema/simple_planning_simulator.schema.json") }}
 
 ### Vehicle Model Parameters
 

simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml → simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml

@@ -4,14 +4,22 @@
     origin_frame_id: "map"
     vehicle_model_type: "DELAY_STEER_ACC_GEARED"
     initialize_source: "INITIAL_POSE_TOPIC"
+    initial_engage_state: True
+    enable_road_slope_simulation: False
     timer_sampling_time_ms: 25
     add_measurement_noise: False
+    pos_noise_stddev: 1e-2
+    vel_noise_stddev: 1e-2
+    rpy_noise_stddev: 1e-4
+    steer_noise_stddev: 1e-4
     vel_lim: 30.0
     vel_rate_lim: 30.0
     steer_lim: 0.6
     steer_rate_lim: 6.28
     acc_time_delay: 0.1
     acc_time_constant: 0.1
+    vel_time_delay: 0.25
+    vel_time_constant: 0.5
     steer_time_delay: 0.1
     steer_time_constant: 0.1
     steer_dead_band: 0.0

simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py

@@ -25,76 +25,69 @@
 
 
 def launch_setup(context, *args, **kwargs):
-    # vehicle information param path
+    # vehicle information config path
     vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context)
     with open(vehicle_info_param_path, "r") as f:
         vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
-    vehicle_characteristics_param_path = LaunchConfiguration(
-        "vehicle_characteristics_param_file"
-    ).perform(context)
-    with open(vehicle_characteristics_param_path, "r") as f:
-        vehicle_characteristics_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-
     simulator_model_param_path = LaunchConfiguration("simulator_model_param_file").perform(context)
     simulator_model_param = launch_ros.parameter_descriptions.ParameterFile(
         param_file=simulator_model_param_path, allow_substs=True
     )
 
     # Base remappings
     remappings = [
         ("input/vector_map", "/map/vector_map"),
         ("input/initialpose", "/initialpose3d"),
         ("input/ackermann_control_command", "/control/command/control_cmd"),
         ("input/actuation_command", "/control/command/actuation_cmd"),
         ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
         ("input/gear_command", "/control/command/gear_cmd"),
         ("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
         ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
         ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
         ("input/trajectory", "/planning/scenario_planning/trajectory"),
         ("input/engage", "/vehicle/engage"),
         ("input/control_mode_request", "/control/control_mode_request"),
         ("output/twist", "/vehicle/status/velocity_status"),
         ("output/imu", "/sensing/imu/imu_data"),
         ("output/steering", "/vehicle/status/steering_status"),
         ("output/gear_report", "/vehicle/status/gear_status"),
         ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
         ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
         ("output/control_mode_report", "/vehicle/status/control_mode"),
         ("output/actuation_status", "/vehicle/status/actuation_status"),
     ]
 
     # Additional remappings
     if LaunchConfiguration("motion_publish_mode").perform(context) == "pose_only":
         remappings.extend(
             [
                 ("output/odometry", "/simulation/debug/localization/kinematic_state"),
                 ("output/acceleration", "/simulation/debug/localization/acceleration"),
                 ("output/pose", "/localization/pose_estimator/pose_with_covariance"),
             ]
         )
     elif LaunchConfiguration("motion_publish_mode").perform(context) == "full_motion":
         remappings.extend(
             [
                 ("output/odometry", "/localization/kinematic_state"),
                 ("output/acceleration", "/localization/acceleration"),
                 (
                     "output/pose",
                     "/simulation/debug/localization/pose_estimator/pose_with_covariance",
                 ),
             ]
         )
 
     simple_planning_simulator_node = Node(
         package="simple_planning_simulator",
         executable="simple_planning_simulator_exe",
         name="simple_planning_simulator",
         namespace="simulation",
         output="screen",
         parameters=[
             vehicle_info_param,
-            vehicle_characteristics_param,
             simulator_model_param,
             {
                 "initial_engage_state": LaunchConfiguration("initial_engage_state"),
@@ -148,20 +141,11 @@
         "path to the parameter file of vehicle information",
     )
 
-    add_launch_arg(
-        "vehicle_characteristics_param_file",
-        [
-            FindPackageShare("simple_planning_simulator"),
-            "/param/vehicle_characteristics.param.yaml",
-        ],
-        "path to config file for vehicle characteristics",
-    )
-
     add_launch_arg(
         "simulator_model_param_file",
         [
             FindPackageShare("simple_planning_simulator"),
-            "/param/simple_planning_simulator_default.param.yaml",
+            "/config/simple_planning_simulator.param.yaml",
         ],
         "path to config file for simulator_model",
     )

simulator/simple_planning_simulator/schema/simple_planning_simulator.schema.json

@@ -0,0 +1,197 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Simple Planning Simulator Node",
+  "type": "object",
+  "definitions": {
+    "simple_planning_simulator": {
+      "type": "object",
+      "properties": {
+        "simulated_frame_id": {
+          "type": "string",
+          "description": "set to the child_frame_id in output tf",
+          "default": "base_link"
+        },
+        "origin_frame_id": {
+          "type": "string",
+          "description": "set to the frame_id in output tf",
+          "default": "map"
+        },
+        "vehicle_model_type": {
+          "type": "string",
+          "description": "Vehicle Model Parameters setting.",
+          "default": "DELAY_STEER_ACC_GEARED",
+          "enum": [
+            "IDEAL_STEER_VEL",
+            "IDEAL_STEER_ACC",
+            "IDEAL_STEER_ACC_GEARED",
+            "DELAY_STEER_VEL",
+            "DELAY_STEER_ACC",
+            "DELAY_STEER_ACC_GEARED"
+          ]
+        },
+        "initialize_source": {
+          "type": "string",
+          "description": "If 'ORIGIN', the initial pose is set at (0,0,0). If 'INITIAL_POSE_TOPIC', node will wait until the 'input/initialpose' topic is published.",
+          "default": "INITIAL_POSE_TOPIC",
+          "enum": ["ORIGIN", "INITIAL_POSE_TOPIC"]
+        },
+        "initial_engage_state": {
+          "type": "boolean",
+          "description": "initial engage state setting",
+          "default": "true"
+        },
+        "enable_road_slope_simulation": {
+          "type": "boolean",
+          "description": "Enable road slope simulation or not.",
+          "default": "false"
+        },
+        "timer_sampling_time_ms": {
+          "type": "integer",
+          "description": "Setting timer for sampling time [ms].",
+          "default": "25",
+          "exclusiveMinimum": 0
+        },
+        "add_measurement_noise": {
+          "type": "boolean",
+          "description": "If true, the Gaussian noise is added to the simulated results.",
+          "default": "false"
+        },
+        "pos_noise_stddev": {
+          "type": "number",
+          "description": "Standard deviation for position noise",
+          "default": "1e-2",
+          "minimum": 0.0
+        },
+        "vel_noise_stddev": {
+          "type": "number",
+          "description": "Standard deviation for longitudinal velocity noise",
+          "default": "1e-2",
+          "minimum": 0.0
+        },
+        "rpy_noise_stddev": {
+          "type": "number",
+          "description": "Standard deviation for Euler angle noise",
+          "default": "1e-4",
+          "minimum": 0.0
+        },
+        "steer_noise_stddev": {
+          "type": "number",
+          "description": "Standard deviation for steering angle noise",
+          "default": "1e-4",
+          "minimum": 0.0
+        },
+        "vel_lim": {
+          "type": "number",
+          "description": "limit of velocity [m/s]",
+          "default": "30.0",
+          "exclusiveMinimum": 0.0
+        },
+        "vel_rate_lim": {
+          "type": "number",
+          "description": "limit of velocity change rate [m/ss]",
+          "default": "30.0",
+          "minimum": 0.0
+        },
+        "steer_lim": {
+          "type": "number",
+          "description": "limit of steering angle [rad]",
+          "default": "0.6"
+        },
+        "steer_rate_lim": {
+          "type": "number",
+          "description": "limit of steering angle change rate [rad/s]",
+          "default": "6.28",
+          "minimum": 0.0
+        },
+        "acc_time_delay": {
+          "type": "number",
+          "description": "dead time for the acceleration input [s]",
+          "default": "0.1",
+          "minimum": 0.0
+        },
+        "acc_time_constant": {
+          "type": "number",
+          "description": "time constant of the 1st-order acceleration dynamics [s]",
+          "default": "0.1",
+          "minimum": 0.0
+        },
+        "vel_time_delay": {
+          "type": "number",
+          "description": "dead time for the velocity input [s]",
+          "default": "0.25",
+          "minimum": 0.0
+        },
+        "vel_time_constant": {
+          "type": "number",
+          "description": "time constant of the 1st-order velocity dynamics [s]",
+          "default": "0.5",
+          "minimum": 0.0
+        },
+        "steer_time_delay": {
+          "type": "number",
+          "description": "dead time for the steering input [s]",
+          "default": "0.1",
+          "minimum": 0.0
+        },
+        "steer_time_constant": {
+          "type": "number",
+          "description": "time constant of the 1st-order steering dynamics [s]",
+          "default": "0.1",
+          "minimum": 0.0
+        },
+        "x_stddev": {
+          "type": "number",
+          "description": "x standard deviation for dummy covariance in map coordinate",
+          "default": "0.0001",
+          "minimum": 0.0
+        },
+        "y_stddev": {
+          "type": "number",
+          "description": "y standard deviation for dummy covariance in map coordinate",
+          "default": "0.0001",
+          "minimum": 0.0
+        }
+      },
+      "required": [
+        "simulated_frame_id",
+        "origin_frame_id",
+        "vehicle_model_type",
+        "initialize_source",
+        "initial_engage_state",
+        "enable_road_slope_simulation",
+        "timer_sampling_time_ms",
+        "add_measurement_noise",
+        "pos_noise_stddev",
+        "vel_noise_stddev",
+        "rpy_noise_stddev",
+        "steer_noise_stddev",
+        "vel_lim",
+        "vel_rate_lim",
+        "steer_lim",
+        "steer_rate_lim",
+        "acc_time_delay",
+        "acc_time_constant",
+        "vel_time_delay",
+        "vel_time_constant",
+        "steer_time_delay",
+        "steer_time_constant",
+        "x_stddev",
+        "y_stddev"
+      ]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/simple_planning_simulator"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}