From 06ee4be50363689ea536ffdac7624bf066c7f3cf Mon Sep 17 00:00:00 2001 From: PhoebeWu21 Date: Thu, 30 Nov 2023 09:40:17 +0800 Subject: [PATCH 1/6] refactor(pose_initializer): rework parameters Signed-off-by: PhoebeWu21 --- localization/pose_initializer/README.md | 9 +- .../config/pose_initializer.param.yaml | 5 ++ .../launch/pose_initializer.launch.xml | 10 --- .../schema/pose_initializer.schema.json | 85 +++++++++++++++++++ 4 files changed, 91 insertions(+), 18 deletions(-) create mode 100644 localization/pose_initializer/schema/pose_initializer.schema.json diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 961f3a8e3293c..7006becf00c2f 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -13,14 +13,7 @@ This node depends on the map height fitter library. ### Parameters -| Name | Type | Description | -| --------------------- | ---- | ---------------------------------------------------------------------------------------- | -| `ekf_enabled` | bool | If true, EKF localizer is activated. | -| `ndt_enabled` | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. | -| `stop_check_enabled` | bool | If true, initialization is accepted only when the vehicle is stopped. | -| `stop_check_duration` | bool | The duration used for the stop check above. | -| `gnss_enabled` | bool | If true, use the GNSS pose when no pose is specified. | -| `gnss_pose_timeout` | bool | The duration that the GNSS pose is valid. | +{{ json_to_markdown("localization/pose_initializer/schema/pose_initializer.schema.json") }} ### Services diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index a05cc7c35cb1a..76e7a8cf58081 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -2,6 +2,11 @@ ros__parameters: gnss_pose_timeout: 3.0 # [sec] stop_check_duration: 3.0 # [sec] + ekf_enabled: true + gnss_enabled: true + yabloc_enabled: true + ndt_enabled: true + stop_check_enabled: true # from gnss gnss_particle_covariance: diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 43e8d48c4fda0..6390d81f868fb 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -1,20 +1,10 @@ - - - - - - - - - - diff --git a/localization/pose_initializer/schema/pose_initializer.schema.json b/localization/pose_initializer/schema/pose_initializer.schema.json new file mode 100644 index 0000000000000..85a72e0787db8 --- /dev/null +++ b/localization/pose_initializer/schema/pose_initializer.schema.json @@ -0,0 +1,85 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Pose Initializer Node", + "type": "object", + "definitions": { + "pose_initializer": { + "type": "object", + "properties": { + "gnss_pose_timeout": { + "type": "number", + "description": "The duration that the GNSS pose is valid. [sec]", + "default": "3.0", + "minimum": 0.0 + }, + "stop_check_enabled": { + "type": "boolean", + "description": "If true, initialization is accepted only when the vehicle is stopped.", + "default": "true" + }, + "stop_check_duration": { + "type": "number", + "description": "The duration used for the stop check above. [sec]", + "default": "3.0", + "minimum": 0.0 + }, + "ekf_enabled": { + "type": "boolean", + "description": "If true, EKF localizer is activated.", + "default": "true" + }, + "gnss_enabled": { + "type": "boolean", + "description": "If true, use the GNSS pose when no pose is specified.", + "default": "true" + }, + "yabloc_enabled": { + "type": "boolean", + "description": "If true, YabLocModule is used.", + "default": "true" + }, + "ndt_enabled": { + "type": "boolean", + "description": "If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through.", + "default": "true" + }, + "gnss_particle_covariance": { + "type": "array", + "description": "gnss particle covariance", + "default": "[1.0, 0.0, 0.0, 0.0, 0.0, 0.0,\n 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,\n 0.0, 0.0, 0.0, 0.0, 0.0, 10.0]" + }, + "output_pose_covariance": { + "type": "array", + "description": "output pose covariance", + "default": "[1.0, 0.0, 0.0, 0.0, 0.0, 0.0,\n 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,\n 0.0, 0.0, 0.0, 0.0, 0.0, 0.2]" + } + }, + "required": [ + "gnss_pose_timeout", + "stop_check_duration", + "ekf_enabled", + "gnss_enabled", + "yabloc_enabled", + "ndt_enabled", + "stop_check_enabled", + "gnss_particle_covariance", + "output_pose_covariance" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pose_initializer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} From a626d53773de9852eceac2625b1449e179e2fb99 Mon Sep 17 00:00:00 2001 From: PhoebeWu21 Date: Tue, 5 Dec 2023 16:16:36 +0800 Subject: [PATCH 2/6] refactor(pose_initializer): rework parameters Signed-off-by: PhoebeWu21 --- localization/pose_initializer/README.md | 9 ++++++ .../config/pose_initializer.param.yaml | 5 ---- .../launch/pose_initializer.launch.xml | 10 +++++++ .../schema/pose_initializer.schema.json | 30 ------------------- 4 files changed, 19 insertions(+), 35 deletions(-) diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 7006becf00c2f..3063e13c10226 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -13,6 +13,15 @@ This node depends on the map height fitter library. ### Parameters +| Name | Type | Description | +| --------------------- | ---- | ---------------------------------------------------------------------------------------- | +| `ekf_enabled` | bool | If true, EKF localizer is activated. | +| `ndt_enabled` | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. | +| `stop_check_enabled` | bool | If true, initialization is accepted only when the vehicle is stopped. | +| `stop_check_duration` | bool | The duration used for the stop check above. | +| `gnss_enabled` | bool | If true, use the GNSS pose when no pose is specified. | +| `gnss_pose_timeout` | bool | The duration that the GNSS pose is valid. | + {{ json_to_markdown("localization/pose_initializer/schema/pose_initializer.schema.json") }} ### Services diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index 76e7a8cf58081..a05cc7c35cb1a 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -2,11 +2,6 @@ ros__parameters: gnss_pose_timeout: 3.0 # [sec] stop_check_duration: 3.0 # [sec] - ekf_enabled: true - gnss_enabled: true - yabloc_enabled: true - ndt_enabled: true - stop_check_enabled: true # from gnss gnss_particle_covariance: diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 6390d81f868fb..43e8d48c4fda0 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -1,10 +1,20 @@ + + + + + + + + + + diff --git a/localization/pose_initializer/schema/pose_initializer.schema.json b/localization/pose_initializer/schema/pose_initializer.schema.json index 85a72e0787db8..e6fe352353d5d 100644 --- a/localization/pose_initializer/schema/pose_initializer.schema.json +++ b/localization/pose_initializer/schema/pose_initializer.schema.json @@ -12,37 +12,12 @@ "default": "3.0", "minimum": 0.0 }, - "stop_check_enabled": { - "type": "boolean", - "description": "If true, initialization is accepted only when the vehicle is stopped.", - "default": "true" - }, "stop_check_duration": { "type": "number", "description": "The duration used for the stop check above. [sec]", "default": "3.0", "minimum": 0.0 }, - "ekf_enabled": { - "type": "boolean", - "description": "If true, EKF localizer is activated.", - "default": "true" - }, - "gnss_enabled": { - "type": "boolean", - "description": "If true, use the GNSS pose when no pose is specified.", - "default": "true" - }, - "yabloc_enabled": { - "type": "boolean", - "description": "If true, YabLocModule is used.", - "default": "true" - }, - "ndt_enabled": { - "type": "boolean", - "description": "If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through.", - "default": "true" - }, "gnss_particle_covariance": { "type": "array", "description": "gnss particle covariance", @@ -57,11 +32,6 @@ "required": [ "gnss_pose_timeout", "stop_check_duration", - "ekf_enabled", - "gnss_enabled", - "yabloc_enabled", - "ndt_enabled", - "stop_check_enabled", "gnss_particle_covariance", "output_pose_covariance" ], From 4959c5174646c72c55cdff3df011b84873dec582 Mon Sep 17 00:00:00 2001 From: PhoebeWu21 Date: Mon, 5 Feb 2024 15:30:34 +0800 Subject: [PATCH 3/6] refactor(pose_initializer): rework parameters Signed-off-by: PhoebeWu21 --- .../config/pose_initializer.param.yaml | 5 ++++ .../launch/pose_initializer.launch.xml | 7 +---- .../schema/pose_initializer.schema.json | 30 +++++++++++++++++++ 3 files changed, 36 insertions(+), 6 deletions(-) diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index a05cc7c35cb1a..88acbfb52611d 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -2,6 +2,11 @@ ros__parameters: gnss_pose_timeout: 3.0 # [sec] stop_check_duration: 3.0 # [sec] + ekf_enabled: $(var ekf_enabled) + gnss_enabled: $(var gnss_enabled) + yabloc_enabled: $(var yabloc_enabled) + ndt_enabled: $(var ndt_enabled) + stop_check_enabled: $(var stop_check_enabled) # from gnss gnss_particle_covariance: diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 43e8d48c4fda0..4526506ce7e35 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -9,12 +9,7 @@ - - - - - - + /> diff --git a/localization/pose_initializer/schema/pose_initializer.schema.json b/localization/pose_initializer/schema/pose_initializer.schema.json index e6fe352353d5d..85a72e0787db8 100644 --- a/localization/pose_initializer/schema/pose_initializer.schema.json +++ b/localization/pose_initializer/schema/pose_initializer.schema.json @@ -12,12 +12,37 @@ "default": "3.0", "minimum": 0.0 }, + "stop_check_enabled": { + "type": "boolean", + "description": "If true, initialization is accepted only when the vehicle is stopped.", + "default": "true" + }, "stop_check_duration": { "type": "number", "description": "The duration used for the stop check above. [sec]", "default": "3.0", "minimum": 0.0 }, + "ekf_enabled": { + "type": "boolean", + "description": "If true, EKF localizer is activated.", + "default": "true" + }, + "gnss_enabled": { + "type": "boolean", + "description": "If true, use the GNSS pose when no pose is specified.", + "default": "true" + }, + "yabloc_enabled": { + "type": "boolean", + "description": "If true, YabLocModule is used.", + "default": "true" + }, + "ndt_enabled": { + "type": "boolean", + "description": "If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through.", + "default": "true" + }, "gnss_particle_covariance": { "type": "array", "description": "gnss particle covariance", @@ -32,6 +57,11 @@ "required": [ "gnss_pose_timeout", "stop_check_duration", + "ekf_enabled", + "gnss_enabled", + "yabloc_enabled", + "ndt_enabled", + "stop_check_enabled", "gnss_particle_covariance", "output_pose_covariance" ], From 9ae08f2f2c4b612bb018264e6ca886a1e29b2fbf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 5 Feb 2024 07:34:41 +0000 Subject: [PATCH 4/6] style(pre-commit): autofix --- .../pose_initializer/launch/pose_initializer.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 4526506ce7e35..ff8b0acfd2ba1 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -9,8 +9,8 @@ - /> - + + /> From e962d9b8720329053ac3d2c0257bc9a2e1af4263 Mon Sep 17 00:00:00 2001 From: PhoebeWu21 Date: Mon, 5 Feb 2024 15:37:12 +0800 Subject: [PATCH 5/6] refactor(pose_initializer): rework parameters Signed-off-by: PhoebeWu21 --- .../pose_initializer/launch/pose_initializer.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 4526506ce7e35..e82ad3893116e 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -9,7 +9,7 @@ - /> + From 03599016d7975563cc5cadceee20583137f01fa0 Mon Sep 17 00:00:00 2001 From: PhoebeWu21 Date: Tue, 6 Feb 2024 11:10:20 +0800 Subject: [PATCH 6/6] refactor(pose_initializer): rework parameters Signed-off-by: PhoebeWu21 --- .../schema/pose_initializer.schema.json | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/localization/pose_initializer/schema/pose_initializer.schema.json b/localization/pose_initializer/schema/pose_initializer.schema.json index 85a72e0787db8..14a286f059030 100644 --- a/localization/pose_initializer/schema/pose_initializer.schema.json +++ b/localization/pose_initializer/schema/pose_initializer.schema.json @@ -13,7 +13,7 @@ "minimum": 0.0 }, "stop_check_enabled": { - "type": "boolean", + "type": "string", "description": "If true, initialization is accepted only when the vehicle is stopped.", "default": "true" }, @@ -24,22 +24,22 @@ "minimum": 0.0 }, "ekf_enabled": { - "type": "boolean", + "type": "string", "description": "If true, EKF localizer is activated.", "default": "true" }, "gnss_enabled": { - "type": "boolean", + "type": "string", "description": "If true, use the GNSS pose when no pose is specified.", "default": "true" }, "yabloc_enabled": { - "type": "boolean", + "type": "string", "description": "If true, YabLocModule is used.", "default": "true" }, "ndt_enabled": { - "type": "boolean", + "type": "string", "description": "If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through.", "default": "true" },