From 589e0a98e991d3ce78d0ab50c99862e5fd6e63a6 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 9 Nov 2023 13:56:20 +0900 Subject: [PATCH 1/3] refactor(start_planner): support new interface (#5526) Update getCurrentStatus() function in StartPlannerModule.cpp Signed-off-by: kyoichi-sugahara --- .../src/scene_module/start_planner/start_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index bd042f8ecef4d..77a290835fb31 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -172,7 +172,7 @@ bool StartPlannerModule::isExecutionRequested() const bool StartPlannerModule::isModuleRunning() const { - return current_state_ == ModuleStatus::RUNNING; + return getCurrentStatus() == ModuleStatus::RUNNING; } bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const From a3e87cc537554e18732c0ebd86ae39d7b9d14a85 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 9 Nov 2023 14:20:23 +0900 Subject: [PATCH 2/3] refactor(goal_planner): support new interface (#5525) Signed-off-by: satoshi-ota --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 6d34ce4ecde38..69c1fb911c4f9 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -296,7 +296,7 @@ void GoalPlannerModule::processOnExit() bool GoalPlannerModule::isExecutionRequested() const { - if (current_state_ == ModuleStatus::RUNNING) { + if (getCurrentStatus() == ModuleStatus::RUNNING) { return true; } From 0e471b3ba67c02fe3c399fe0e9c23b411fb00271 Mon Sep 17 00:00:00 2001 From: Yuqi Huai <34195403+YuqiHuai@users.noreply.github.com> Date: Wed, 8 Nov 2023 22:08:13 -0800 Subject: [PATCH 3/3] refactor(gyro_odometer): rework parameters (#5243) * refactor(gyro_odometer): rework parameters Signed-off-by: Yuqi Huai * style(pre-commit): autofix * doc(gyro_odometer): remove copyright --------- Signed-off-by: Yuqi Huai Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/gyro_odometer/CMakeLists.txt | 1 + localization/gyro_odometer/README.md | 5 +-- .../config/gyro_odometer.param.yaml | 4 ++ .../launch/gyro_odometer.launch.xml | 6 +-- .../schema/gyro_odometer.schema.json | 38 +++++++++++++++++++ .../gyro_odometer/src/gyro_odometer_core.cpp | 4 +- 6 files changed, 48 insertions(+), 10 deletions(-) create mode 100644 localization/gyro_odometer/config/gyro_odometer.param.yaml create mode 100644 localization/gyro_odometer/schema/gyro_odometer.schema.json diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 59a5e1121b22f..57589837dd529 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -32,4 +32,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index ec50a113af4d9..e9e390010f084 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -21,10 +21,7 @@ ## Parameters -| Parameter | Type | Description | -| --------------------- | ------ | -------------------------------- | -| `output_frame` | String | output's frame id | -| `message_timeout_sec` | Double | delay tolerance time for message | +{{ json_to_markdown("localization/gyro_odometer/schema/gyro_odometer.schema.json") }} ## Assumptions / Known limits diff --git a/localization/gyro_odometer/config/gyro_odometer.param.yaml b/localization/gyro_odometer/config/gyro_odometer.param.yaml new file mode 100644 index 0000000000000..420a9b0d80582 --- /dev/null +++ b/localization/gyro_odometer/config/gyro_odometer.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + output_frame: "base_link" + message_timeout_sec: 0.2 diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index aeb505270b2cc..21aff3e10d26c 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -9,8 +9,7 @@ - - + @@ -23,7 +22,6 @@ - - + diff --git a/localization/gyro_odometer/schema/gyro_odometer.schema.json b/localization/gyro_odometer/schema/gyro_odometer.schema.json new file mode 100644 index 0000000000000..189df2a2f63f3 --- /dev/null +++ b/localization/gyro_odometer/schema/gyro_odometer.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for gyro odometer", + "type": "object", + "definitions": { + "gyro_odometer": { + "type": "object", + "properties": { + "output_frame": { + "type": "string", + "description": "output's frame id", + "default": "base_link" + }, + "message_timeout_sec": { + "type": "number", + "description": "delay tolerance time for message", + "default": 0.2 + } + }, + "required": ["output_frame", "message_timeout_sec"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/gyro_odometer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index c14c48ffb2046..5de0ecd7cdc0a 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -102,8 +102,8 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) : Node("gyro_odometer", options), - output_frame_(declare_parameter("output_frame", "base_link")), - message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)), + output_frame_(declare_parameter("output_frame")), + message_timeout_sec_(declare_parameter("message_timeout_sec")), vehicle_twist_arrived_(false), imu_arrived_(false) {