From 6e0e6c09f422063d72a727229bf34daffef279a9 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Tue, 14 Jan 2025 19:23:19 +0900 Subject: [PATCH 01/59] fix(behavior_velocity_planner_common): fix unregister process (#9913) Signed-off-by: yuki-takagi-66 --- .../behavior_velocity_planner_common/scene_module_interface.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index d8d640a078267..a891b011dbab8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -228,6 +228,7 @@ class SceneModuleManagerInterface auto itr = scene_modules_.begin(); while (itr != scene_modules_.end()) { if (isModuleExpired(*itr)) { + registered_module_id_set_.erase((*itr)->getModuleId()); itr = scene_modules_.erase(itr); } else { itr++; From 5f88055f48c885733cfa6217c811a17a21470a36 Mon Sep 17 00:00:00 2001 From: Kirill Glinskiy <56448851+PurplePegasuss@users.noreply.github.com> Date: Tue, 14 Jan 2025 19:10:07 +0300 Subject: [PATCH 02/59] docs(autoware_mission_planner): added odometry input in README.md (#7281) added odometry input in README.md Signed-off-by: Ryohsuke Mitsudome --- planning/autoware_mission_planner/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md index b5993d0106add..720ecd976f65f 100644 --- a/planning/autoware_mission_planner/README.md +++ b/planning/autoware_mission_planner/README.md @@ -53,6 +53,7 @@ It distributes route requests and planning results according to current MRM oper | `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | | `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | | `input/operation_mode_state` | autoware_adapi_v1_msgs/OperationModeState | operation mode state | +| `input/odometry` | nav_msgs/msg/Odometry | vehicle odometry | ### Publications From 951b1df19373a57c813ece9dd15dcd6a6f6bb0cf Mon Sep 17 00:00:00 2001 From: cyn-liu <104069308+cyn-liu@users.noreply.github.com> Date: Wed, 15 Jan 2025 00:22:37 +0800 Subject: [PATCH 03/59] fix(autoware_tensorrt_yolox): modify tensorrt_yolox_node name (#9156) Signed-off-by: liu cui --- .../launch/multiple_yolox.launch.xml | 16 ++++++++++++++++ .../launch/yolox_s_plus_opt.launch.xml | 6 ++++-- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml b/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml index a7952b12414b1..606fd1299cf35 100644 --- a/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml @@ -11,34 +11,50 @@ + + + + + + + + + + + + + + + + diff --git a/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index 0096a219c8573..6d40905d78127 100644 --- a/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -1,6 +1,8 @@ + + @@ -15,13 +17,13 @@ - + - + From bc0dcccbec3131cea2bde0761f811f971f0f10e4 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Wed, 15 Jan 2025 09:25:04 +0900 Subject: [PATCH 04/59] test(external_velocity_limit_selector): add node test (#8944) add node smoke test Signed-off-by: Maxime CLEMENT --- .../CMakeLists.txt | 7 + .../package.xml | 1 + .../external_velocity_limit_selector_node.cpp | 1 - ...nal_velocity_limit_selector_node_launch.py | 178 ++++++++++++++++++ 4 files changed, 186 insertions(+), 1 deletion(-) create mode 100644 planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py diff --git a/planning/autoware_external_velocity_limit_selector/CMakeLists.txt b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt index ca758d1262b48..aa8d07d91135e 100644 --- a/planning/autoware_external_velocity_limit_selector/CMakeLists.txt +++ b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt @@ -21,6 +21,13 @@ rclcpp_components_register_node(external_velocity_limit_selector_node EXECUTABLE external_velocity_limit_selector ) +if(BUILD_TESTING) + add_launch_test( + test/test_external_velocity_limit_selector_node_launch.py + TIMEOUT "30" + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/planning/autoware_external_velocity_limit_selector/package.xml b/planning/autoware_external_velocity_limit_selector/package.xml index b75a4fab72d7c..5bfd3fa936d50 100644 --- a/planning/autoware_external_velocity_limit_selector/package.xml +++ b/planning/autoware_external_velocity_limit_selector/package.xml @@ -28,6 +28,7 @@ ament_lint_auto autoware_lint_common + ros_testing ament_cmake diff --git a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index cf42763fc6b60..cc2b134900a31 100644 --- a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -14,7 +14,6 @@ #include "autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" -#include #include #include #include diff --git a/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py b/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py new file mode 100644 index 0000000000000..1c7e882fbc81f --- /dev/null +++ b/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python3 + +# Copyright 2024 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.logging import get_logger +import launch_testing +import pytest +from rcl_interfaces.msg import Parameter +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters +import rclpy +import rclpy.qos +from tier4_planning_msgs.msg import VelocityLimit +from tier4_planning_msgs.msg import VelocityLimitClearCommand + +logger = get_logger(__name__) + + +@pytest.mark.launch_test +def generate_test_description(): + test_external_velocity_limit_selector_launch_file = os.path.join( + get_package_share_directory("autoware_external_velocity_limit_selector"), + "launch", + "external_velocity_limit_selector.launch.xml", + ) + external_velocity_limit_selector = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_external_velocity_limit_selector_launch_file), + ) + + return launch.LaunchDescription( + [ + external_velocity_limit_selector, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestExternalVelocityLimitSelector(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def velocity_limit_callback(self, msg): + self.msg_buffer_ = msg + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + qos = rclpy.qos.QoSProfile(depth=1, durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL) + self.pub_api_limit_ = self.test_node.create_publisher( + VelocityLimit, "/planning/scenario_planning/max_velocity_default", qos + ) + self.pub_internal_limit_ = self.test_node.create_publisher( + VelocityLimit, "/planning/scenario_planning/max_velocity_candidates", qos + ) + self.pub_clear_limit_ = self.test_node.create_publisher( + VelocityLimitClearCommand, "/planning/scenario_planning/clear_velocity_limit", qos + ) + self.msg_buffer_ = None + self.velocity_limit_output_ = None + self.test_node.create_subscription( + VelocityLimit, + "/planning/scenario_planning/max_velocity", + self.velocity_limit_callback, + 1, + ) + + def wait_for_output(self): + while not self.msg_buffer_: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + self.velocity_limit_output_ = self.msg_buffer_ + self.msg_buffer_ = None + + def tearDown(self): + self.test_node.destroy_node() + + def update_max_vel_param(self, max_vel): + set_params_client = self.test_node.create_client( + SetParameters, "/external_velocity_limit_selector/set_parameters" + ) + while not set_params_client.wait_for_service(timeout_sec=1.0): + continue + set_params_request = SetParameters.Request() + set_params_request.parameters = [ + Parameter( + name="max_vel", + value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=max_vel), + ), + ] + future = set_params_client.call_async(set_params_request) + rclpy.spin_until_future_complete(self.test_node, future) + + if future.result() is None: + self.test_node.get_logger().error( + "Exception while calling service: %r" % future.exception() + ) + raise self.failureException("setting of initial parameters failed") + + @staticmethod + def make_velocity_limit_msg(vel): + msg = VelocityLimit() + msg.use_constraints = False + msg.max_velocity = vel + return msg + + def test_external_velocity_limit_selector_node(self): + self.update_max_vel_param(15.0) + # clear velocity limit to trigger first output + clear_cmd = VelocityLimitClearCommand(command=True) + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + # velocity limit is 0 before any limit is set + self.assertEqual(self.velocity_limit_output_.max_velocity, 0.0) + + # Send velocity limits + # new API velocity limit: higher than the node param -> limit is set to the param value + api_limit = self.make_velocity_limit_msg(20.0) + self.pub_api_limit_.publish(api_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 15.0) + # new API velocity limit + api_limit = self.make_velocity_limit_msg(10.0) + self.pub_api_limit_.publish(api_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 10.0) + # new INTERNAL velocity limit + internal_limit = self.make_velocity_limit_msg(5.0) + self.pub_internal_limit_.publish(internal_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 5.0) + # CLEAR: back to API velocity limit + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 10.0) + # lower the max_vel node parameter + self.update_max_vel_param(2.5) + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 2.5) + # velocity limit set by internal limit is no longer applied since above max_vel parameter + internal_limit = self.make_velocity_limit_msg(5.0) + self.pub_internal_limit_.publish(internal_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 2.5) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) From bc409a755a2d0abce18f73c9d84aa05d851cdc5d Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Wed, 15 Jan 2025 09:32:12 +0900 Subject: [PATCH 05/59] feat(remaining_distance_time_calculator): integrate generate_parameter_library (#8826) * add parameter description Signed-off-by: mitukou1109 * use parameter listener Signed-off-by: mitukou1109 * supress deprecated error Signed-off-by: mitukou1109 * change scope of compile option to private Signed-off-by: mitukou1109 --------- Signed-off-by: mitukou1109 --- .../CMakeLists.txt | 12 ++++++++++++ .../package.xml | 1 + ...emaining_distance_time_calculator_parameters.yaml | 4 ++++ .../src/remaining_distance_time_calculator_node.cpp | 6 ++++-- .../src/remaining_distance_time_calculator_node.hpp | 8 ++------ 5 files changed, 23 insertions(+), 8 deletions(-) create mode 100644 planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml diff --git a/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt b/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt index d29a153a0ce5d..31fc4b1572f33 100644 --- a/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt +++ b/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt @@ -4,10 +4,22 @@ project(autoware_remaining_distance_time_calculator) find_package(autoware_cmake REQUIRED) autoware_package() +generate_parameter_library(remaining_distance_time_calculator_parameters + param/remaining_distance_time_calculator_parameters.yaml +) + ament_auto_add_library(${PROJECT_NAME} SHARED src/remaining_distance_time_calculator_node.cpp ) +target_link_libraries(${PROJECT_NAME} + remaining_distance_time_calculator_parameters +) + +target_compile_options(${PROJECT_NAME} PRIVATE + -Wno-error=deprecated-declarations +) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::remaining_distance_time_calculator::RemainingDistanceTimeCalculatorNode" EXECUTABLE ${PROJECT_NAME}_node diff --git a/planning/autoware_remaining_distance_time_calculator/package.xml b/planning/autoware_remaining_distance_time_calculator/package.xml index 924e6d62d6aba..568f677a0c13d 100644 --- a/planning/autoware_remaining_distance_time_calculator/package.xml +++ b/planning/autoware_remaining_distance_time_calculator/package.xml @@ -18,6 +18,7 @@ autoware_route_handler autoware_test_utils autoware_universe_utils + generate_parameter_library geometry_msgs nav_msgs rclcpp diff --git a/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml b/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml new file mode 100644 index 0000000000000..0f1b1b9efbb6d --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml @@ -0,0 +1,4 @@ +remaining_distance_time_calculator: + update_rate: + type: double + description: Timer callback period. [Hz] diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp index 7932a2531185f..74f3f2f43077d 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -65,9 +65,11 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( "~/output/mission_remaining_distance_time", rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable()); - node_param_.update_rate = declare_parameter("update_rate"); + param_listener_ = std::make_shared<::remaining_distance_time_calculator::ParamListener>( + this->get_node_parameters_interface()); + const auto param = param_listener_->get_params(); - const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + const auto period_ns = rclcpp::Rate(param.update_rate).period(); timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&RemainingDistanceTimeCalculatorNode::on_timer, this)); } diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp index b7af8861bf524..8a38941d383ee 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -39,11 +40,6 @@ namespace autoware::remaining_distance_time_calculator { -struct NodeParam -{ - double update_rate{0.0}; -}; - class RemainingDistanceTimeCalculatorNode : public rclcpp::Node { public: @@ -86,7 +82,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node double remaining_time_; // Parameter - NodeParam node_param_; + std::shared_ptr<::remaining_distance_time_calculator::ParamListener> param_listener_; // Callbacks void on_timer(); From 87d7988fae3ac1b135caa6de0c6911da8e4e154d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BF=83=E5=88=9A?= <90366790+liuXinGangChina@users.noreply.github.com> Date: Wed, 15 Jan 2025 09:41:50 +0800 Subject: [PATCH 06/59] feat(autoware_localization_util): porting from universe to core, autoware_localization_util, remove from universe repo (#9888) task: porting from universe to core, autoware_localization_util, remove from universe repo : v0.0 Signed-off-by: liuXinGangChina --- .../autoware_localization_util/CHANGELOG.rst | 51 ---- .../autoware_localization_util/CMakeLists.txt | 29 -- .../autoware_localization_util/README.md | 5 - .../localization_util/covariance_ellipse.hpp | 44 --- .../localization_util/matrix_type.hpp | 26 -- .../localization_util/smart_pose_buffer.hpp | 71 ----- .../tree_structured_parzen_estimator.hpp | 87 ------ .../autoware/localization_util/util_func.hpp | 88 ------ .../autoware_localization_util/package.xml | 35 --- .../src/covariance_ellipse.cpp | 92 ------- .../src/smart_pose_buffer.cpp | 158 ----------- .../src/tree_structured_parzen_estimator.cpp | 185 ------------- .../src/util_func.cpp | 257 ------------------ .../test/test_smart_pose_buffer.cpp | 109 -------- .../test/test_tpe.cpp | 75 ----- 15 files changed, 1312 deletions(-) delete mode 100644 localization/autoware_localization_util/CHANGELOG.rst delete mode 100644 localization/autoware_localization_util/CMakeLists.txt delete mode 100644 localization/autoware_localization_util/README.md delete mode 100644 localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp delete mode 100644 localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp delete mode 100644 localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp delete mode 100644 localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp delete mode 100644 localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp delete mode 100644 localization/autoware_localization_util/package.xml delete mode 100644 localization/autoware_localization_util/src/covariance_ellipse.cpp delete mode 100644 localization/autoware_localization_util/src/smart_pose_buffer.cpp delete mode 100644 localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp delete mode 100644 localization/autoware_localization_util/src/util_func.cpp delete mode 100644 localization/autoware_localization_util/test/test_smart_pose_buffer.cpp delete mode 100644 localization/autoware_localization_util/test/test_tpe.cpp diff --git a/localization/autoware_localization_util/CHANGELOG.rst b/localization/autoware_localization_util/CHANGELOG.rst deleted file mode 100644 index be40e3ee8560e..0000000000000 --- a/localization/autoware_localization_util/CHANGELOG.rst +++ /dev/null @@ -1,51 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_localization_util -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - localization (`#9567 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(localization_util)!: prefix package and namespace with autoware (`#8922 `_) - add autoware prefix to localization_util -* Contributors: Masaki Baba, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt deleted file mode 100644 index de62633124f3d..0000000000000 --- a/localization/autoware_localization_util/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_localization_util) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/util_func.cpp - src/smart_pose_buffer.cpp - src/tree_structured_parzen_estimator.cpp - src/covariance_ellipse.cpp -) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_smart_pose_buffer - test/test_smart_pose_buffer.cpp - src/smart_pose_buffer.cpp - ) - - ament_auto_add_gtest(test_tpe - test/test_tpe.cpp - src/tree_structured_parzen_estimator.cpp - ) -endif() - -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md deleted file mode 100644 index f7fddd9eebf05..0000000000000 --- a/localization/autoware_localization_util/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# autoware_localization_util - -`autoware_localization_util` is a localization utility package. - -This package does not have a node, it is just a library. diff --git a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp deleted file mode 100644 index abd0af46856b0..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2024 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ - -#include - -#include -#include - -namespace autoware::localization_util -{ - -struct Ellipse -{ - double long_radius; - double short_radius; - double yaw; - Eigen::Matrix2d P; - double size_lateral_direction; -}; - -Ellipse calculate_xy_ellipse( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale); - -visualization_msgs::msg::Marker create_ellipse_marker( - const Ellipse & ellipse, const std_msgs::msg::Header & header, - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); - -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp deleted file mode 100644 index bda6ff19f2867..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2021 TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ - -#include - -namespace autoware::localization_util -{ -using Matrix6d = Eigen::Matrix; -using RowMatrixXd = Eigen::Matrix; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp deleted file mode 100644 index 8c10506c36753..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ - -#include "autoware/localization_util/util_func.hpp" - -#include - -#include - -#include - -namespace autoware::localization_util -{ -class SmartPoseBuffer -{ -private: - using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - -public: - struct InterpolateResult - { - PoseWithCovarianceStamped old_pose; - PoseWithCovarianceStamped new_pose; - PoseWithCovarianceStamped interpolated_pose; - }; - - SmartPoseBuffer() = delete; - SmartPoseBuffer( - const rclcpp::Logger & logger, const double & pose_timeout_sec, - const double & pose_distance_tolerance_meters); - - std::optional interpolate(const rclcpp::Time & target_ros_time); - - void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr); - - void pop_old(const rclcpp::Time & target_ros_time); - - void clear(); - -private: - rclcpp::Logger logger_; - std::deque pose_buffer_; - std::mutex mutex_; // This mutex is for pose_buffer_ - - const double pose_timeout_sec_; - const double pose_distance_tolerance_meters_; - - [[nodiscard]] bool validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const; - [[nodiscard]] bool validate_position_difference( - const geometry_msgs::msg::Point & target_point, - const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; -}; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp deleted file mode 100644 index ddf7625c202ec..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ - -/* -A implementation of tree-structured parzen estimator (TPE) -See below pdf for the TPE algorithm detail. -https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf - -Optuna is also used as a reference for implementation. -https://github.com/optuna/optuna -*/ - -#include -#include -#include - -namespace autoware::localization_util -{ -class TreeStructuredParzenEstimator -{ -public: - using Input = std::vector; - using Score = double; - struct Trial - { - Input input; - Score score; - }; - - enum Direction { - MINIMIZE = 0, - MAXIMIZE = 1, - }; - - enum Index { - TRANS_X = 0, - TRANS_Y = 1, - TRANS_Z = 2, - ANGLE_X = 3, - ANGLE_Y = 4, - ANGLE_Z = 5, - INDEX_NUM = 6, - }; - - TreeStructuredParzenEstimator() = delete; - TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, - std::vector sample_stddev); - void add_trial(const Trial & trial); - [[nodiscard]] Input get_next_input() const; - -private: - static constexpr double max_good_rate = 0.10; - static constexpr int64_t n_ei_candidates = 100; - - static std::mt19937_64 engine; - - [[nodiscard]] double compute_log_likelihood_ratio(const Input & input) const; - [[nodiscard]] static double log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma); - - std::vector trials_; - int64_t above_num_; - const Direction direction_; - const int64_t n_startup_trials_; - const int64_t input_dimension_; - const std::vector sample_mean_; - const std::vector sample_stddev_; - Input base_stddev_; -}; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp deleted file mode 100644 index bef9968f34b6f..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ - -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - -#include -#include -#include -#include -#include -#include - -namespace autoware::localization_util -{ -// ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA exchange_color_crc(double x); - -double calc_diff_for_radian(const double lhs_rad, const double rhs_rad); - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose); -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose); -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - -geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( - const double r_rad, const double p_rad, const double y_rad); -geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( - const double r_deg, const double p_deg, const double y_deg); - -geometry_msgs::msg::Twist calc_twist( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, - const rclcpp::Time & time_stamp); - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); - -Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose); -Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose); -geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix); -Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos); - -template -T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) -{ - T output; - tf2::doTransform(input, output, transform); - return output; -} - -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); - -void output_pose_with_cov_to_log( - const rclcpp::Logger & logger, const std::string & prefix, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); - -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/autoware_localization_util/package.xml b/localization/autoware_localization_util/package.xml deleted file mode 100644 index 89ad7c24840dd..0000000000000 --- a/localization/autoware_localization_util/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - autoware_localization_util - 0.40.0 - The autoware_localization_util package - Yamato Ando - Masahiro Sakamoto - Shintaro Sakoda - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Ryu Yamamoto - Apache License 2.0 - Yamato Ando - - ament_cmake_auto - autoware_cmake - - diagnostic_msgs - geometry_msgs - rclcpp - std_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - visualization_msgs - - ament_cmake_cppcheck - ament_lint_auto - - - ament_cmake - - diff --git a/localization/autoware_localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp deleted file mode 100644 index 847f89e0da9b3..0000000000000 --- a/localization/autoware_localization_util/src/covariance_ellipse.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright 2024 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/covariance_ellipse.hpp" - -#include - -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -namespace autoware::localization_util -{ - -Ellipse calculate_xy_ellipse( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale) -{ - // input geometry_msgs::PoseWithCovariance contain 6x6 matrix - Eigen::Matrix2d xy_covariance; - const auto cov = pose_with_covariance.covariance; - xy_covariance(0, 0) = cov[0 * 6 + 0]; - xy_covariance(0, 1) = cov[0 * 6 + 1]; - xy_covariance(1, 0) = cov[1 * 6 + 0]; - xy_covariance(1, 1) = cov[1 * 6 + 1]; - - Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); - - Ellipse ellipse; - - // eigen values and vectors are sorted in ascending order - ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1)); - ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0)); - - // principal component vector - const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); - ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x()); - - // ellipse size along lateral direction (body-frame) - ellipse.P = xy_covariance; - const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation); - const Eigen::Matrix2d & p_inv = ellipse.P.inverse(); - Eigen::MatrixXd e(2, 1); - e(0, 0) = std::cos(yaw_vehicle); - e(1, 0) = std::sin(yaw_vehicle); - const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant()); - ellipse.size_lateral_direction = scale * d; - - return ellipse; -} - -visualization_msgs::msg::Marker create_ellipse_marker( - const Ellipse & ellipse, const std_msgs::msg::Header & header, - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) -{ - tf2::Quaternion quat; - quat.setEuler(0, 0, ellipse.yaw); - - const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); - const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); - visualization_msgs::msg::Marker marker; - marker.header = header; - marker.ns = "error_ellipse"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = pose_with_covariance.pose; - marker.pose.orientation = tf2::toMsg(quat); - marker.scale.x = ellipse_long_radius * 2; - marker.scale.y = ellipse_short_radius * 2; - marker.scale.z = 0.01; - marker.color.a = 0.1; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - return marker; -} - -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/smart_pose_buffer.cpp b/localization/autoware_localization_util/src/smart_pose_buffer.cpp deleted file mode 100644 index 3b529d68cf6c5..0000000000000 --- a/localization/autoware_localization_util/src/smart_pose_buffer.cpp +++ /dev/null @@ -1,158 +0,0 @@ -// Copyright 2023- Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/smart_pose_buffer.hpp" - -namespace autoware::localization_util -{ -SmartPoseBuffer::SmartPoseBuffer( - const rclcpp::Logger & logger, const double & pose_timeout_sec, - const double & pose_distance_tolerance_meters) -: logger_(logger), - pose_timeout_sec_(pose_timeout_sec), - pose_distance_tolerance_meters_(pose_distance_tolerance_meters) -{ -} - -std::optional SmartPoseBuffer::interpolate( - const rclcpp::Time & target_ros_time) -{ - InterpolateResult result; - - { - std::lock_guard lock(mutex_); - - if (pose_buffer_.size() < 2) { - RCLCPP_INFO(logger_, "pose_buffer_.size() < 2"); - return std::nullopt; - } - - const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; - const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; - - if (target_ros_time < time_first) { - RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp"); - return std::nullopt; - } - - // [time_last < target_ros_time] is acceptable here. - // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest - // timestamp of buffered pose (often EKF). - // However, if the timestamp difference is too large, - // it will later be rejected by validate_time_stamp_difference. - - // get the nearest poses - result.old_pose = *pose_buffer_.front(); - for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { - result.new_pose = *pose_cov_msg_ptr; - const rclcpp::Time pose_time_stamp = result.new_pose.header.stamp; - if (pose_time_stamp > target_ros_time) { - break; - } - result.old_pose = *pose_cov_msg_ptr; - } - } - - // check the time stamp - const bool is_old_pose_valid = validate_time_stamp_difference( - result.old_pose.header.stamp, target_ros_time, pose_timeout_sec_); - const bool is_new_pose_valid = validate_time_stamp_difference( - result.new_pose.header.stamp, target_ros_time, pose_timeout_sec_); - - // check the position jumping (ex. immediately after the initial pose estimation) - const bool is_pose_diff_valid = validate_position_difference( - result.old_pose.pose.pose.position, result.new_pose.pose.pose.position, - pose_distance_tolerance_meters_); - - // all validations must be true - if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { - return std::nullopt; - } - - // interpolate the pose - const geometry_msgs::msg::PoseStamped interpolated_pose_msg = - interpolate_pose(result.old_pose, result.new_pose, target_ros_time); - result.interpolated_pose.header = interpolated_pose_msg.header; - result.interpolated_pose.pose.pose = interpolated_pose_msg.pose; - // This does not interpolate the covariance. - // interpolated_pose.pose.covariance is always set to old_pose.covariance - result.interpolated_pose.pose.covariance = result.old_pose.pose.covariance; - return result; -} - -void SmartPoseBuffer::push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr) -{ - std::lock_guard lock(mutex_); - if (!pose_buffer_.empty()) { - // Check for non-chronological timestamp order - // This situation can arise when replaying a rosbag multiple times - const rclcpp::Time end_time = pose_buffer_.back()->header.stamp; - const rclcpp::Time msg_time = pose_msg_ptr->header.stamp; - if (msg_time < end_time) { - // Clear the buffer if timestamps are reversed to maintain chronological order - pose_buffer_.clear(); - } - } - pose_buffer_.push_back(pose_msg_ptr); -} - -void SmartPoseBuffer::pop_old(const rclcpp::Time & target_ros_time) -{ - std::lock_guard lock(mutex_); - while (!pose_buffer_.empty()) { - if (rclcpp::Time(pose_buffer_.front()->header.stamp) >= target_ros_time) { - break; - } - pose_buffer_.pop_front(); - } -} - -void SmartPoseBuffer::clear() -{ - std::lock_guard lock(mutex_); - pose_buffer_.clear(); -} - -bool SmartPoseBuffer::validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const -{ - const double dt = std::abs((target_time - reference_time).seconds()); - const bool success = dt < time_tolerance_sec; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " - "difference is %lf[sec] (the tolerance is %lf[sec]).", - reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); - } - return success; -} - -bool SmartPoseBuffer::validate_position_difference( - const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, - const double distance_tolerance_m_) const -{ - const double distance = norm(target_point, reference_point); - const bool success = distance < distance_tolerance_m_; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The distance from reference position to target position is %lf[m] (the " - "tolerance is %lf[m]).", - distance, distance_tolerance_m_); - } - return success; -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp deleted file mode 100644 index e9f0318d1e06d..0000000000000 --- a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp +++ /dev/null @@ -1,185 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::localization_util -{ -// random number generator -std::mt19937_64 TreeStructuredParzenEstimator::engine(0); - -TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, - std::vector sample_stddev) -: above_num_(0), - direction_(direction), - n_startup_trials_(n_startup_trials), - input_dimension_(INDEX_NUM), - sample_mean_(std::move(sample_mean)), - sample_stddev_(std::move(sample_stddev)) -{ - if (sample_mean_.size() != ANGLE_Z) { - std::cerr << "sample_mean size is invalid" << std::endl; - throw std::runtime_error("sample_mean size is invalid"); - } - if (sample_stddev_.size() != ANGLE_Z) { - std::cerr << "sample_stddev size is invalid" << std::endl; - throw std::runtime_error("sample_stddev size is invalid"); - } - // base_stddev_ is defined as the stable convergence range of ndt_scan_matcher. - base_stddev_.resize(input_dimension_); - base_stddev_[TRANS_X] = 0.25; // [m] - base_stddev_[TRANS_Y] = 0.25; // [m] - base_stddev_[TRANS_Z] = 0.25; // [m] - base_stddev_[ANGLE_X] = 1.0 / 180.0 * M_PI; // [rad] - base_stddev_[ANGLE_Y] = 1.0 / 180.0 * M_PI; // [rad] - base_stddev_[ANGLE_Z] = 2.5 / 180.0 * M_PI; // [rad] -} - -void TreeStructuredParzenEstimator::add_trial(const Trial & trial) -{ - trials_.push_back(trial); - std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { - return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); - }); - above_num_ = std::min( - {static_cast(10), - static_cast(static_cast(trials_.size()) * max_good_rate)}); -} - -TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const -{ - std::normal_distribution dist_normal_trans_x( - sample_mean_[TRANS_X], sample_stddev_[TRANS_X]); - std::normal_distribution dist_normal_trans_y( - sample_mean_[TRANS_Y], sample_stddev_[TRANS_Y]); - std::normal_distribution dist_normal_trans_z( - sample_mean_[TRANS_Z], sample_stddev_[TRANS_Z]); - std::normal_distribution dist_normal_angle_x( - sample_mean_[ANGLE_X], sample_stddev_[ANGLE_X]); - std::normal_distribution dist_normal_angle_y( - sample_mean_[ANGLE_Y], sample_stddev_[ANGLE_Y]); - std::uniform_real_distribution dist_uniform_angle_z(-M_PI, M_PI); - - if (static_cast(trials_.size()) < n_startup_trials_ || above_num_ == 0) { - // Random sampling based on prior until the number of trials reaches `n_startup_trials_`. - Input input(input_dimension_); - input[TRANS_X] = dist_normal_trans_x(engine); - input[TRANS_Y] = dist_normal_trans_y(engine); - input[TRANS_Z] = dist_normal_trans_z(engine); - input[ANGLE_X] = dist_normal_angle_x(engine); - input[ANGLE_Y] = dist_normal_angle_y(engine); - input[ANGLE_Z] = dist_uniform_angle_z(engine); - return input; - } - - Input best_input; - double best_log_likelihood_ratio = std::numeric_limits::lowest(); - for (int64_t i = 0; i < n_ei_candidates; i++) { - Input input(input_dimension_); - input[TRANS_X] = dist_normal_trans_x(engine); - input[TRANS_Y] = dist_normal_trans_y(engine); - input[TRANS_Z] = dist_normal_trans_z(engine); - input[ANGLE_X] = dist_normal_angle_x(engine); - input[ANGLE_Y] = dist_normal_angle_y(engine); - input[ANGLE_Z] = dist_uniform_angle_z(engine); - const double log_likelihood_ratio = compute_log_likelihood_ratio(input); - if (log_likelihood_ratio > best_log_likelihood_ratio) { - best_log_likelihood_ratio = log_likelihood_ratio; - best_input = input; - } - } - return best_input; -} - -double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const -{ - const auto n = static_cast(trials_.size()); - - // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to - // select best sample. - std::vector above_logs; - std::vector below_logs; - - for (int64_t i = 0; i < n; i++) { - const double log_p = log_gaussian_pdf(input, trials_[i].input, base_stddev_); - if (i < above_num_) { - const double w = 1.0 / static_cast(above_num_); - const double log_w = std::log(w); - above_logs.push_back(log_p + log_w); - } else { - const double w = 1.0 / static_cast(n - above_num_); - const double log_w = std::log(w); - below_logs.push_back(log_p + log_w); - } - } - - auto log_sum_exp = [](const std::vector & log_vec) { - const double max = *std::max_element(log_vec.begin(), log_vec.end()); - double sum = std::accumulate( - log_vec.begin(), log_vec.end(), 0.0, - [max](double total, double log_v) { return total + std::exp(log_v - max); }); - return max + std::log(sum); - }; - - const double above = log_sum_exp(above_logs); - const double below = log_sum_exp(below_logs); - - // Multiply by a constant so that the score near the "below sample" becomes lower. - // cspell:disable-line TODO(Shintaro Sakoda): It's theoretically incorrect, consider it again - // later. - const double r = above - below * 5.0; - return r; -} - -double TreeStructuredParzenEstimator::log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma) -{ - const double log_2pi = std::log(2.0 * M_PI); - auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { - return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); - }; - - const auto n = static_cast(input.size()); - - double result = 0.0; - for (int64_t i = 0; i < n; i++) { - double diff = input[i] - mu[i]; - if (i == ANGLE_Z) { - // Normalize the loop variable to [-pi, pi) - while (diff >= M_PI) { - diff -= 2 * M_PI; - } - while (diff < -M_PI) { - diff += 2 * M_PI; - } - } - // Experimentally, it is better to consider only trans_xy and yaw, so ignore trans_z, angle_x, - // angle_y. - if (i == TRANS_Z || i == ANGLE_X || i == ANGLE_Y) { - continue; - } - result += log_gaussian_pdf_1d(diff, sigma[i]); - } - return result; -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp deleted file mode 100644 index 17187a8d732f0..0000000000000 --- a/localization/autoware_localization_util/src/util_func.cpp +++ /dev/null @@ -1,257 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/util_func.hpp" - -#include "autoware/localization_util/matrix_type.hpp" - -#include -#include - -namespace autoware::localization_util -{ -// ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA exchange_color_crc(double x) -{ - std_msgs::msg::ColorRGBA color; - - x = std::max(x, 0.0); - x = std::min(x, 0.9999); - - if (x <= 0.25) { - color.b = 1.0; - color.g = static_cast(std::sin(x * 2.0 * M_PI)); - color.r = 0; - } else if (x <= 0.5) { - color.b = static_cast(std::sin(x * 2 * M_PI)); - color.g = 1.0; - color.r = 0; - } else if (x <= 0.75) { - color.b = 0; - color.g = 1.0; - color.r = static_cast(-std::sin(x * 2.0 * M_PI)); - } else { - color.b = 0; - color.g = static_cast(-std::sin(x * 2.0 * M_PI)); - color.r = 1.0; - } - color.a = 0.999; - return color; -} - -double calc_diff_for_radian(const double lhs_rad, const double rhs_rad) -{ - double diff_rad = lhs_rad - rhs_rad; - if (diff_rad > M_PI) { - diff_rad = diff_rad - 2 * M_PI; - } else if (diff_rad < -M_PI) { - diff_rad = diff_rad + 2 * M_PI; - } - return diff_rad; -} - -Eigen::Map make_eigen_covariance(const std::array & covariance) -{ - return {covariance.data(), 6, 6}; -} - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; -} - -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose) -{ - return get_rpy(pose.pose); -} - -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) -{ - return get_rpy(pose.pose.pose); -} - -geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( - const double r_rad, const double p_rad, const double y_rad) -{ - tf2::Quaternion q; - q.setRPY(r_rad, p_rad, y_rad); - geometry_msgs::msg::Quaternion quaternion_msg; - quaternion_msg.x = q.x(); - quaternion_msg.y = q.y(); - quaternion_msg.z = q.z(); - quaternion_msg.w = q.w(); - return quaternion_msg; -} - -geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( - const double r_deg, const double p_deg, const double y_deg) -{ - const double r_rad = r_deg * M_PI / 180.0; - const double p_rad = p_deg * M_PI / 180.0; - const double y_rad = y_deg * M_PI / 180.0; - return rpy_rad_to_quaternion(r_rad, p_rad, y_rad); -} - -geometry_msgs::msg::Twist calc_twist( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) -{ - const rclcpp::Duration dt = rclcpp::Time(pose_b.header.stamp) - rclcpp::Time(pose_a.header.stamp); - const double dt_s = dt.seconds(); - - if (dt_s == 0) { - return geometry_msgs::msg::Twist(); - } - - const auto pose_a_rpy = get_rpy(pose_a); - const auto pose_b_rpy = get_rpy(pose_b); - - geometry_msgs::msg::Vector3 diff_xyz; - geometry_msgs::msg::Vector3 diff_rpy; - - diff_xyz.x = pose_b.pose.position.x - pose_a.pose.position.x; - diff_xyz.y = pose_b.pose.position.y - pose_a.pose.position.y; - diff_xyz.z = pose_b.pose.position.z - pose_a.pose.position.z; - diff_rpy.x = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x); - diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y); - diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z); - - geometry_msgs::msg::Twist twist; - twist.linear.x = diff_xyz.x / dt_s; - twist.linear.y = diff_xyz.y / dt_s; - twist.linear.z = diff_xyz.z / dt_s; - twist.angular.x = diff_rpy.x / dt_s; - twist.angular.y = diff_rpy.y / dt_s; - twist.angular.z = diff_rpy.z / dt_s; - - return twist; -} - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, - const rclcpp::Time & time_stamp) -{ - const rclcpp::Time pose_a_time_stamp = pose_a.header.stamp; - const rclcpp::Time pose_b_time_stamp = pose_b.header.stamp; - if ( - (pose_a_time_stamp.seconds() == 0.0) || (pose_b_time_stamp.seconds() == 0.0) || - (time_stamp.seconds() == 0.0)) { - return geometry_msgs::msg::PoseStamped(); - } - - const auto twist = calc_twist(pose_a, pose_b); - const double dt = (time_stamp - pose_a_time_stamp).seconds(); - - const auto pose_a_rpy = get_rpy(pose_a); - - geometry_msgs::msg::Vector3 xyz; - geometry_msgs::msg::Vector3 rpy; - xyz.x = pose_a.pose.position.x + twist.linear.x * dt; - xyz.y = pose_a.pose.position.y + twist.linear.y * dt; - xyz.z = pose_a.pose.position.z + twist.linear.z * dt; - rpy.x = pose_a_rpy.x + twist.angular.x * dt; - rpy.y = pose_a_rpy.y + twist.angular.y * dt; - rpy.z = pose_a_rpy.z + twist.angular.z * dt; - - tf2::Quaternion tf_quaternion; - tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); - - geometry_msgs::msg::PoseStamped pose; - pose.header = pose_a.header; - pose.header.stamp = time_stamp; - pose.pose.position.x = xyz.x; - pose.pose.position.y = xyz.y; - pose.pose.position.z = xyz.z; - pose.pose.orientation = tf2::toMsg(tf_quaternion); - return pose; -} - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp) -{ - geometry_msgs::msg::PoseStamped tmp_pose_a; - tmp_pose_a.header = pose_a.header; - tmp_pose_a.pose = pose_a.pose.pose; - - geometry_msgs::msg::PoseStamped tmp_pose_b; - tmp_pose_b.header = pose_b.header; - tmp_pose_b.pose = pose_b.pose.pose; - - return interpolate_pose(tmp_pose_a, tmp_pose_b, time_stamp); -} - -Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) -{ - Eigen::Affine3d eigen_pose; - tf2::fromMsg(ros_pose, eigen_pose); - return eigen_pose; -} - -Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose) -{ - Eigen::Affine3d eigen_pose_affine = pose_to_affine3d(ros_pose); - Eigen::Matrix4f eigen_pose_matrix = eigen_pose_affine.matrix().cast(); - return eigen_pose_matrix; -} - -Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos) -{ - Eigen::Vector3d eigen_pos; - eigen_pos.x() = ros_pos.x; - eigen_pos.y() = ros_pos.y; - eigen_pos.z() = ros_pos.z; - return eigen_pos; -} - -geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix) -{ - Eigen::Affine3d eigen_pose_affine; - eigen_pose_affine.matrix() = eigen_pose_matrix.cast(); - geometry_msgs::msg::Pose ros_pose = tf2::toMsg(eigen_pose_affine); - return ros_pose; -} - -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) -{ - const double dx = p1.x - p2.x; - const double dy = p1.y - p2.y; - const double dz = p1.z - p2.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); -} - -void output_pose_with_cov_to_log( - const rclcpp::Logger & logger, const std::string & prefix, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) -{ - const Eigen::Map covariance = - make_eigen_covariance(pose_with_cov.pose.covariance); - const geometry_msgs::msg::Pose pose = pose_with_cov.pose.pose; - geometry_msgs::msg::Vector3 rpy = get_rpy(pose); - rpy.x = rpy.x * 180.0 / M_PI; - rpy.y = rpy.y * 180.0 / M_PI; - rpy.z = rpy.z * 180.0 / M_PI; - - RCLCPP_DEBUG_STREAM( - logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," - << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y - << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x - << "," << rpy.y << "," << rpy.z << "," << covariance(0, 0) << "," - << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) - << "," << covariance(4, 4) << "," << covariance(5, 5)); -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp deleted file mode 100644 index d55555682be84..0000000000000 --- a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2023- Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/smart_pose_buffer.hpp" -#include "autoware/localization_util/util_func.hpp" - -#include -#include - -#include -#include - -#include -#include -#include - -using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -using SmartPoseBuffer = autoware::localization_util::SmartPoseBuffer; - -bool compare_pose( - const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) -{ - return pose_a.header.stamp == pose_b.header.stamp && - pose_a.header.frame_id == pose_b.header.frame_id && - pose_a.pose.covariance == pose_b.pose.covariance && - pose_a.pose.pose.position.x == pose_b.pose.pose.position.x && - pose_a.pose.pose.position.y == pose_b.pose.pose.position.y && - pose_a.pose.pose.position.z == pose_b.pose.pose.position.z && - pose_a.pose.pose.orientation.x == pose_b.pose.pose.orientation.x && - pose_a.pose.pose.orientation.y == pose_b.pose.pose.orientation.y && - pose_a.pose.pose.orientation.z == pose_b.pose.pose.orientation.z && - pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; -} - -TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT -{ - rclcpp::Logger logger = rclcpp::get_logger("test_logger"); - const double pose_timeout_sec = 10.0; - const double pose_distance_tolerance_meters = 100.0; - SmartPoseBuffer smart_pose_buffer(logger, pose_timeout_sec, pose_distance_tolerance_meters); - - // first data - PoseWithCovarianceStamped::SharedPtr old_pose_ptr = std::make_shared(); - old_pose_ptr->header.stamp.sec = 10; - old_pose_ptr->header.stamp.nanosec = 0; - old_pose_ptr->pose.pose.position.x = 10.0; - old_pose_ptr->pose.pose.position.y = 20.0; - old_pose_ptr->pose.pose.position.z = 0.0; - old_pose_ptr->pose.pose.orientation = - autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 0.0); - smart_pose_buffer.push_back(old_pose_ptr); - - // second data - PoseWithCovarianceStamped::SharedPtr new_pose_ptr = std::make_shared(); - new_pose_ptr->header.stamp.sec = 20; - new_pose_ptr->header.stamp.nanosec = 0; - new_pose_ptr->pose.pose.position.x = 20.0; - new_pose_ptr->pose.pose.position.y = 40.0; - new_pose_ptr->pose.pose.position.z = 0.0; - new_pose_ptr->pose.pose.orientation = - autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 90.0); - smart_pose_buffer.push_back(new_pose_ptr); - - // interpolate - builtin_interfaces::msg::Time target_ros_time_msg; - target_ros_time_msg.sec = 15; - target_ros_time_msg.nanosec = 0; - const std::optional & interpolate_result = - smart_pose_buffer.interpolate(target_ros_time_msg); - ASSERT_TRUE(interpolate_result.has_value()); - const SmartPoseBuffer::InterpolateResult result = interpolate_result.value(); - - // check old - EXPECT_TRUE(compare_pose(result.old_pose, *old_pose_ptr)); - - // check new - EXPECT_TRUE(compare_pose(result.new_pose, *new_pose_ptr)); - - // check interpolated - EXPECT_EQ(result.interpolated_pose.header.stamp.sec, 15); - EXPECT_EQ(result.interpolated_pose.header.stamp.nanosec, static_cast(0)); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); - const auto rpy = autoware::localization_util::get_rpy(result.interpolated_pose.pose.pose); - EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); - EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); - EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/localization/autoware_localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp deleted file mode 100644 index 2d71a385246b7..0000000000000 --- a/localization/autoware_localization_util/test/test_tpe.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" - -#include - -#include -#include -#include -#include -#include - -using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator; - -TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) -{ - auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { - double value = 0.0; - const auto n = static_cast(input.size()); - for (int64_t i = 0; i < n; i++) { - const double v = input[i] * 10; - value += v * v; - } - return value; - }; - - constexpr int64_t k_outer_trials_num = 20; - constexpr int64_t k_inner_trials_num = 200; - std::cout << std::fixed; - std::vector mean_scores; - std::vector sample_mean(5, 0.0); - std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; - - for (const int64_t n_startup_trials : {k_inner_trials_num, k_inner_trials_num / 2}) { - const std::string method = ((n_startup_trials == k_inner_trials_num) ? "Random" : "TPE"); - - std::vector scores; - for (int64_t i = 0; i < k_outer_trials_num; i++) { - double best_score = std::numeric_limits::lowest(); - TreeStructuredParzenEstimator estimator( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, sample_mean, - sample_stddev); - for (int64_t trial = 0; trial < k_inner_trials_num; trial++) { - const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); - const double score = -sphere_function(input); - estimator.add_trial({input, score}); - best_score = std::max(best_score, score); - } - scores.push_back(best_score); - } - - const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); - const double mean = sum / static_cast(scores.size()); - mean_scores.push_back(mean); - double sq_sum = std::accumulate( - scores.begin(), scores.end(), 0.0, - [mean](double total, double score) { return total + (score - mean) * (score - mean); }); - const double stddev = std::sqrt(sq_sum / static_cast(scores.size())); - - std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; - } - ASSERT_LT(mean_scores[0], mean_scores[1]); -} From 557a71e16695083aa40091b60f17c1daa90a257d Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 15 Jan 2025 10:52:13 +0900 Subject: [PATCH 07/59] fix(dummy_infrastructure): fix bugprone-reserved-identifier (#9919) Signed-off-by: kobayu858 Co-authored-by: kobayu858 --- .../src/dummy_infrastructure_node/dummy_infrastructure_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp b/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp index 485d85c926268..4760c9366da46 100644 --- a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp +++ b/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp @@ -24,7 +24,6 @@ using namespace std::literals; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; -using std::placeholders::_1; namespace dummy_infrastructure { @@ -84,6 +83,7 @@ boost::optional findCommand( DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & node_options) : Node("dummy_infrastructure", node_options) { + using std::placeholders::_1; // Parameter Server set_param_res_ = this->add_on_set_parameters_callback(std::bind(&DummyInfrastructureNode::onSetParam, this, _1)); From 6939a3da23a41781a6d973a8267e2ea1cb3a600d Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Wed, 15 Jan 2025 09:57:09 +0800 Subject: [PATCH 08/59] feat(autoware_qp_interface): porting autoware_qp_interface package to autoware.core (#9824) Signed-off-by: NorahXiong --- common/autoware_qp_interface/CHANGELOG.rst | 81 ---- common/autoware_qp_interface/CMakeLists.txt | 64 --- .../design/qp_interface-design.md | 48 --- .../qp_interface/osqp_csc_matrix_conv.hpp | 46 -- .../autoware/qp_interface/osqp_interface.hpp | 147 ------- .../qp_interface/proxqp_interface.hpp | 57 --- .../autoware/qp_interface/qp_interface.hpp | 61 --- common/autoware_qp_interface/package.xml | 30 -- .../src/osqp_csc_matrix_conv.cpp | 134 ------ .../src/osqp_interface.cpp | 394 ------------------ .../src/proxqp_interface.cpp | 157 ------- .../src/qp_interface.cpp | 70 ---- .../test/test_csc_matrix_conv.cpp | 181 -------- .../test/test_osqp_interface.cpp | 170 -------- .../test/test_proxqp_interface.cpp | 85 ---- 15 files changed, 1725 deletions(-) delete mode 100644 common/autoware_qp_interface/CHANGELOG.rst delete mode 100644 common/autoware_qp_interface/CMakeLists.txt delete mode 100644 common/autoware_qp_interface/design/qp_interface-design.md delete mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp delete mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp delete mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp delete mode 100644 common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp delete mode 100644 common/autoware_qp_interface/package.xml delete mode 100644 common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp delete mode 100644 common/autoware_qp_interface/src/osqp_interface.cpp delete mode 100644 common/autoware_qp_interface/src/proxqp_interface.cpp delete mode 100644 common/autoware_qp_interface/src/qp_interface.cpp delete mode 100644 common/autoware_qp_interface/test/test_csc_matrix_conv.cpp delete mode 100644 common/autoware_qp_interface/test/test_osqp_interface.cpp delete mode 100644 common/autoware_qp_interface/test/test_proxqp_interface.cpp diff --git a/common/autoware_qp_interface/CHANGELOG.rst b/common/autoware_qp_interface/CHANGELOG.rst deleted file mode 100644 index f76dac861e1e6..0000000000000 --- a/common/autoware_qp_interface/CHANGELOG.rst +++ /dev/null @@ -1,81 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_qp_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* fix: fix package names in changelog files (`#9500 `_) -* fix(autoware_osqp_interface): fix clang-tidy errors (`#9440 `_) -* 0.39.0 -* update changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* fix(qp_interface): fix unreadVariable (`#8349 `_) - * fix:unreadVariable - * fix:unreadVariable - * fix:unreadVariable - --------- -* perf(velocity_smoother): use ProxQP for faster optimization (`#8028 `_) - * perf(velocity_smoother): use ProxQP for faster optimization - * consider max_iteration - * disable warm start - * fix test - --------- -* refactor(qp_interface): clean up the code (`#8029 `_) - * refactor qp_interface - * variable names: m_XXX -> XXX\_ - * update - * update - --------- -* fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions (`#7855 `_) - * fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Ryuta Kambe, Takayuki Murooka, Yutaka Kondo, kobayu858 - -0.26.0 (2024-04-03) -------------------- -* feat(qp_interface): support proxqp (`#3699 `_) - * feat(qp_interface): add proxqp interface - * update - --------- -* feat(qp_interface): add new package which will contain various qp solvers (`#3697 `_) -* Contributors: Takayuki Murooka diff --git a/common/autoware_qp_interface/CMakeLists.txt b/common/autoware_qp_interface/CMakeLists.txt deleted file mode 100644 index 1df75d2d719a0..0000000000000 --- a/common/autoware_qp_interface/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_qp_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) -find_package(proxsuite REQUIRED) - -# after find_package(osqp_vendor) in ament_auto_find_build_dependencies -find_package(osqp REQUIRED) -get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) -get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) - -set(QP_INTERFACE_LIB_SRC - src/qp_interface.cpp - src/osqp_interface.cpp - src/osqp_csc_matrix_conv.cpp - src/proxqp_interface.cpp -) - -set(QP_INTERFACE_LIB_HEADERS - include/autoware/qp_interface/qp_interface.hpp - include/autoware/qp_interface/osqp_interface.hpp - include/autoware/qp_interface/osqp_csc_matrix_conv.hpp - include/autoware/qp_interface/proxqp_interface.hpp -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - ${QP_INTERFACE_LIB_SRC} - ${QP_INTERFACE_LIB_HEADERS} -) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) - -target_include_directories(${PROJECT_NAME} - SYSTEM PUBLIC - "${OSQP_INCLUDE_DIR}" - "${EIGEN3_INCLUDE_DIR}" -) - -ament_target_dependencies(${PROJECT_NAME} - Eigen3 - osqp_vendor - proxsuite -) - -# crucial so downstream package builds because osqp_interface exposes osqp.hpp -ament_export_include_directories("${OSQP_INCLUDE_DIR}") -# crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a -ament_export_libraries(osqp::osqp) - -if(BUILD_TESTING) - set(TEST_SOURCES - test/test_osqp_interface.cpp - test/test_csc_matrix_conv.cpp - test/test_proxqp_interface.cpp - ) - set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) - ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/common/autoware_qp_interface/design/qp_interface-design.md b/common/autoware_qp_interface/design/qp_interface-design.md deleted file mode 100644 index edc7fa9845206..0000000000000 --- a/common/autoware_qp_interface/design/qp_interface-design.md +++ /dev/null @@ -1,48 +0,0 @@ -# Interface for QP solvers - -This is the design document for the `autoware_qp_interface` package. - -## Purpose / Use cases - -This packages provides a C++ interface for QP solvers. -Currently, supported QP solvers are - -- [OSQP library](https://osqp.org/docs/solver/index.html) - -## Design - -The class `QPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into -C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the QP workspace dataholder, and runs the optimizer. - -## Inputs / Outputs / API - -The interface can be used in several ways: - -1. Initialize the interface, and load the problem formulation at the optimization call. - - ```cpp - QPInterface qp_interface; - qp_interface.optimize(P, A, q, l, u); - ``` - -2. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. - - ```cpp - QPInterface qp_interface(true); - qp_interface.optimize(P, A, q, l, u); - qp_interface.optimize(P_new, A_new, q_new, l_new, u_new); - ``` - - The optimization results are returned as a vector by the optimization function. - - ```cpp - const auto solution = qp_interface.optimize(); - double x_0 = solution[0]; - double x_1 = solution[1]; - ``` - -## References / External links - -- OSQP library: - -## Related issues diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp deleted file mode 100644 index 9575d9d18c69f..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ -#define AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ - -#include "osqp/glob_opts.h" - -#include - -#include - -namespace autoware::qp_interface -{ -/// \brief Compressed-Column-Sparse Matrix -struct CSC_Matrix -{ - /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector vals_; - /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector row_idxs_; - /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector col_idxs_; -}; - -/// \brief Calculate CSC matrix from Eigen matrix -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); -/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); -/// \brief Print the given CSC matrix to the standard output -void printCSCMatrix(const CSC_Matrix & csc_mat); - -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp deleted file mode 100644 index a5777dd545e9f..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp +++ /dev/null @@ -1,147 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" -#include "autoware/qp_interface/qp_interface.hpp" -#include "osqp/osqp.h" - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -constexpr c_float OSQP_INF = 1e30; - -class OSQPInterface : public QPInterface -{ -public: - /// \brief Constructor without problem formulation - OSQPInterface( - const bool enable_warm_start = false, const int max_iteration = 20000, - const c_float eps_abs = std::numeric_limits::epsilon(), - const c_float eps_rel = std::numeric_limits::epsilon(), const bool polish = true, - const bool verbose = false); - /// \brief Constructor with problem setup - /// \param P: (n,n) matrix defining relations between parameters. - /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q: (n) vector defining the linear cost of the problem. - /// \param l: (m) vector defining the lower bound problem constraint. - /// \param u: (m) vector defining the upper bound problem constraint. - /// \param eps_abs: Absolute convergence tolerance. - OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, - const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon()); - OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, - const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon()); - ~OSQPInterface(); - - static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; - - std::vector optimize( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - int getIterationNumber() const override; - bool isSolved() const override; - std::string getStatus() const override; - - int getPolishStatus() const; - std::vector getDualSolution() const; - - void updateEpsAbs(const double eps_abs) override; - void updateEpsRel(const double eps_rel) override; - void updateVerbose(const bool verbose) override; - - // Updates problem parameters while keeping solution in memory. - // - // Args: - // P_new: (n,n) matrix defining relations between parameters. - // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q_new: (n) vector defining the linear cost of the problem. - // l_new: (m) vector defining the lower bound problem constraint. - // u_new: (m) vector defining the upper bound problem constraint. - void updateP(const Eigen::MatrixXd & P_new); - void updateCscP(const CSC_Matrix & P_csc); - void updateA(const Eigen::MatrixXd & A_new); - void updateCscA(const CSC_Matrix & A_csc); - void updateQ(const std::vector & q_new); - void updateL(const std::vector & l_new); - void updateU(const std::vector & u_new); - void updateBounds(const std::vector & l_new, const std::vector & u_new); - - void updateMaxIter(const int iter); - void updateRhoInterval(const int rho_interval); - void updateRho(const double rho); - void updateAlpha(const double alpha); - void updateScaling(const int scaling); - void updatePolish(const bool polish); - void updatePolishRefinementIteration(const int polish_refine_iter); - void updateCheckTermination(const int check_termination); - - /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(latest_work_info_.iter); } - /// \brief Get the status message for the latest problem solved - inline std::string getStatusMessage() const - { - return static_cast(latest_work_info_.status); - } - /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return latest_work_info_.run_time; } - /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return latest_work_info_.obj_val; } - /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return exitflag_; } - - // Setter functions for warm start - bool setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables); - bool setPrimalVariables(const std::vector & primal_variables); - bool setDualVariables(const std::vector & dual_variables); - -private: - std::unique_ptr> work_; - std::unique_ptr settings_; - std::unique_ptr data_; - // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo latest_work_info_; - // Number of parameters to optimize - int64_t param_n_; - // Flag to check if the current work exists - bool work__initialized = false; - // Exitflag - int64_t exitflag_; - - void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) override; - - void initializeCSCProblemImpl( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - std::vector optimizeImpl() override; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp deleted file mode 100644 index 324da4b18c6fd..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ - -#include "autoware/qp_interface/qp_interface.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -class ProxQPInterface : public QPInterface -{ -public: - explicit ProxQPInterface( - const bool enable_warm_start, const int max_iteration, const double eps_abs, - const double eps_rel, const bool verbose = false); - - int getIterationNumber() const override; - bool isSolved() const override; - std::string getStatus() const override; - - void updateEpsAbs(const double eps_abs) override; - void updateEpsRel(const double eps_rel) override; - void updateVerbose(const bool verbose) override; - -private: - proxsuite::proxqp::Settings settings_{}; - std::shared_ptr> qp_ptr_{nullptr}; - - void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) override; - - std::vector optimizeImpl() override; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp deleted file mode 100644 index be3c598512bf6..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ - -#include - -#include -#include -#include - -namespace autoware::qp_interface -{ -class QPInterface -{ -public: - explicit QPInterface(const bool enable_warm_start) : enable_warm_start_(enable_warm_start) {} - - std::vector optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - virtual bool isSolved() const = 0; - virtual int getIterationNumber() const = 0; - virtual std::string getStatus() const = 0; - - virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; - virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; - virtual void updateVerbose([[maybe_unused]] const bool verbose) {} - -protected: - bool enable_warm_start_{false}; - - void initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - virtual void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) = 0; - - virtual std::vector optimizeImpl() = 0; - - std::optional variables_num_{std::nullopt}; - std::optional constraints_num_{std::nullopt}; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/package.xml b/common/autoware_qp_interface/package.xml deleted file mode 100644 index b61d03a36db72..0000000000000 --- a/common/autoware_qp_interface/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_qp_interface - 0.40.0 - Interface for the QP solvers - Takayuki Murooka - Fumiya Watanabe - Maxime CLEMENT - Satoshi Ota - Apache 2.0 - - ament_cmake_auto - autoware_cmake - - eigen - osqp_vendor - proxsuite - rclcpp - rclcpp_components - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp b/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp deleted file mode 100644 index 15314a9e4a5fa..0000000000000 --- a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp +++ /dev/null @@ -1,134 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" - -#include -#include - -#include -#include -#include - -namespace autoware::qp_interface -{ -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i < rows; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - if (rows != cols) { - throw std::invalid_argument("Matrix must be square (n, n)"); - } - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - Eigen::Index trap_last_idx = 0; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - trap_last_idx += 1; - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -void printCSCMatrix(const CSC_Matrix & csc_mat) -{ - std::cout << "["; - for (const c_float & val : csc_mat.vals_) { - std::cout << val << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & row : csc_mat.row_idxs_) { - std::cout << row << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & col : csc_mat.col_idxs_) { - std::cout << col << ", "; - } - std::cout << "]\n"; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/osqp_interface.cpp b/common/autoware_qp_interface/src/osqp_interface.cpp deleted file mode 100644 index fbb8e71f4c251..0000000000000 --- a/common/autoware_qp_interface/src/osqp_interface.cpp +++ /dev/null @@ -1,394 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_interface.hpp" - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -OSQPInterface::OSQPInterface( - const bool enable_warm_start, const int max_iteration, const c_float eps_abs, - const c_float eps_rel, const bool polish, const bool verbose) -: QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter} -{ - settings_ = std::make_unique(); - data_ = std::make_unique(); - - if (settings_) { - osqp_set_default_settings(settings_.get()); - settings_->alpha = 1.6; // Change alpha parameter - settings_->eps_rel = eps_rel; - settings_->eps_abs = eps_abs; - settings_->eps_prim_inf = 1.0E-4; - settings_->eps_dual_inf = 1.0E-4; - settings_->warm_start = enable_warm_start; - settings_->max_iter = max_iteration; - settings_->verbose = verbose; - settings_->polish = polish; - } - exitflag_ = 0; -} - -OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const bool enable_warm_start, - const c_float eps_abs) -: OSQPInterface(enable_warm_start, eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const bool enable_warm_start, - const c_float eps_abs) -: OSQPInterface(enable_warm_start, eps_abs) -{ - initializeCSCProblemImpl(P, A, q, l, u); -} - -OSQPInterface::~OSQPInterface() -{ - if (data_->P) free(data_->P); - if (data_->A) free(data_->A); -} - -void OSQPInterface::initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - initializeCSCProblemImpl(P_csc, A_csc, q, l, u); -} - -void OSQPInterface::initializeCSCProblemImpl( - CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector u_tmp(u.begin(), u.end()); - double * q_dyn = q_tmp.data(); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - - /********************** - * OBJECTIVE FUNCTION - **********************/ - param_n_ = static_cast(q.size()); - data_->m = static_cast(l.size()); - - /***************** - * POPULATE DATA - *****************/ - data_->n = param_n_; - if (data_->P) free(data_->P); - data_->P = csc_matrix( - data_->n, data_->n, static_cast(P_csc.vals_.size()), P_csc.vals_.data(), - P_csc.row_idxs_.data(), P_csc.col_idxs_.data()); - data_->q = q_dyn; - if (data_->A) free(data_->A); - data_->A = csc_matrix( - data_->m, data_->n, static_cast(A_csc.vals_.size()), A_csc.vals_.data(), - A_csc.row_idxs_.data(), A_csc.col_idxs_.data()); - data_->l = l_dyn; - data_->u = u_dyn; - - // Setup workspace - OSQPWorkspace * workspace; - exitflag_ = osqp_setup(&workspace, data_.get(), settings_.get()); - work_.reset(workspace); - work__initialized = true; -} - -void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept -{ - if (ptr != nullptr) { - osqp_cleanup(ptr); - } -} - -void OSQPInterface::updateEpsAbs(const double eps_abs) -{ - settings_->eps_abs = eps_abs; // for default setting - if (work__initialized) { - osqp_update_eps_abs(work_.get(), eps_abs); // for current work - } -} - -void OSQPInterface::updateEpsRel(const double eps_rel) -{ - settings_->eps_rel = eps_rel; // for default setting - if (work__initialized) { - osqp_update_eps_rel(work_.get(), eps_rel); // for current work - } -} - -void OSQPInterface::updateMaxIter(const int max_iter) -{ - settings_->max_iter = max_iter; // for default setting - if (work__initialized) { - osqp_update_max_iter(work_.get(), max_iter); // for current work - } -} - -void OSQPInterface::updateVerbose(const bool is_verbose) -{ - settings_->verbose = is_verbose; // for default setting - if (work__initialized) { - osqp_update_verbose(work_.get(), is_verbose); // for current work - } -} - -void OSQPInterface::updateRhoInterval(const int rho_interval) -{ - settings_->adaptive_rho_interval = rho_interval; // for default setting -} - -void OSQPInterface::updateRho(const double rho) -{ - settings_->rho = rho; - if (work__initialized) { - osqp_update_rho(work_.get(), rho); - } -} - -void OSQPInterface::updateAlpha(const double alpha) -{ - settings_->alpha = alpha; - if (work__initialized) { - osqp_update_alpha(work_.get(), alpha); - } -} - -void OSQPInterface::updateScaling(const int scaling) -{ - settings_->scaling = scaling; -} - -void OSQPInterface::updatePolish(const bool polish) -{ - settings_->polish = polish; - if (work__initialized) { - osqp_update_polish(work_.get(), polish); - } -} - -void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) -{ - if (polish_refine_iter < 0) { - std::cerr << "Polish refinement iterations must be positive" << std::endl; - return; - } - - settings_->polish_refine_iter = polish_refine_iter; - if (work__initialized) { - osqp_update_polish_refine_iter(work_.get(), polish_refine_iter); - } -} - -void OSQPInterface::updateCheckTermination(const int check_termination) -{ - if (check_termination < 0) { - std::cerr << "Check termination must be positive" << std::endl; - return; - } - - settings_->check_termination = check_termination; - if (work__initialized) { - osqp_update_check_termination(work_.get(), check_termination); - } -} - -bool OSQPInterface::setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables) -{ - return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); -} - -bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) -{ - if (!work__initialized) { - return false; - } - - const auto result = osqp_warm_start_x(work_.get(), primal_variables.data()); - if (result != 0) { - std::cerr << "Failed to set primal variables for warm start" << std::endl; - return false; - } - - return true; -} - -bool OSQPInterface::setDualVariables(const std::vector & dual_variables) -{ - if (!work__initialized) { - return false; - } - - const auto result = osqp_warm_start_y(work_.get(), dual_variables.data()); - if (result != 0) { - std::cerr << "Failed to set dual variables for warm start" << std::endl; - return false; - } - - return true; -} - -void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) -{ - /* - // Transform 'P' into an 'upper trapezoidal matrix' - Eigen::MatrixXd P_trap = P_new.triangularView(); - // Transform 'P' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix P_sparse = P_trap.sparseView(); - double *P_val_ptr = P_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int P_elem_N = P_sparse.nonZeros(); - */ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); -} - -void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) -{ - osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); -} - -void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) -{ - /* - // Transform 'A' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix A_sparse = A_new.sparseView(); - double *A_val_ptr = A_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int A_elem_N = A_sparse.nonZeros(); - */ - CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); - return; -} - -void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) -{ - osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); -} - -void OSQPInterface::updateQ(const std::vector & q_new) -{ - std::vector q_tmp(q_new.begin(), q_new.end()); - double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(work_.get(), q_dyn); -} - -void OSQPInterface::updateL(const std::vector & l_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(work_.get(), l_dyn); -} - -void OSQPInterface::updateU(const std::vector & u_new) -{ - std::vector u_tmp(u_new.begin(), u_new.end()); - double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(work_.get(), u_dyn); -} - -void OSQPInterface::updateBounds( - const std::vector & l_new, const std::vector & u_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - std::vector u_tmp(u_new.begin(), u_new.end()); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - osqp_update_bounds(work_.get(), l_dyn, u_dyn); -} - -int OSQPInterface::getIterationNumber() const -{ - return work_->info->iter; -} - -std::string OSQPInterface::getStatus() const -{ - return "OSQP_SOLVED"; -} - -bool OSQPInterface::isSolved() const -{ - return latest_work_info_.status_val == 1; -} - -int OSQPInterface::getPolishStatus() const -{ - return static_cast(latest_work_info_.status_polish); -} - -std::vector OSQPInterface::getDualSolution() const -{ - double * sol_y = work_->solution->y; - std::vector dual_solution(sol_y, sol_y + data_->m); - return dual_solution; -} - -std::vector OSQPInterface::optimizeImpl() -{ - osqp_solve(work_.get()); - - double * sol_x = work_->solution->x; - std::vector sol_primal(sol_x, sol_x + param_n_); - - latest_work_info_ = *(work_->info); - - if (!enable_warm_start_) { - work_.reset(); - work__initialized = false; - } - - return sol_primal; -} - -std::vector OSQPInterface::optimize( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - initializeCSCProblemImpl(P, A, q, l, u); - const auto result = optimizeImpl(); - - // show polish status if not successful - const int status_polish = static_cast(latest_work_info_.status_polish); - if (status_polish != 1) { - const auto msg = status_polish == 0 ? "unperformed" - : status_polish == -1 ? "unsuccessful" - : "unknown"; - std::cerr << "osqp polish process failed : " << msg << ". The result may be inaccurate" - << std::endl; - } - - return result; -} - -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/proxqp_interface.cpp b/common/autoware_qp_interface/src/proxqp_interface.cpp deleted file mode 100644 index a0ebbf0db0510..0000000000000 --- a/common/autoware_qp_interface/src/proxqp_interface.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/proxqp_interface.hpp" - -#include -#include -#include - -namespace autoware::qp_interface -{ -using proxsuite::proxqp::QPSolverOutput; - -ProxQPInterface::ProxQPInterface( - const bool enable_warm_start, const int max_iteration, const double eps_abs, const double eps_rel, - const bool verbose) -: QPInterface(enable_warm_start) -{ - settings_.max_iter = max_iteration; - settings_.eps_abs = eps_abs; - settings_.eps_rel = eps_rel; - settings_.verbose = verbose; -} - -void ProxQPInterface::initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - const size_t variables_num = q.size(); - const size_t constraints_num = l.size(); - - const bool enable_warm_start = [&]() { - if ( - !enable_warm_start_ // Warm start is designated - || !qp_ptr_ // QP pointer is initialized - // The number of variables is the same as the previous one. - || !variables_num_ || - *variables_num_ != variables_num - // The number of constraints is the same as the previous one - || !constraints_num_ || *constraints_num_ != constraints_num) { - return false; - } - return true; - }(); - - if (!enable_warm_start) { - qp_ptr_ = std::make_shared>( - variables_num, 0, constraints_num); - } - - settings_.initial_guess = - enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT - : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; - - qp_ptr_->settings = settings_; - - Eigen::SparseMatrix P_sparse(variables_num, constraints_num); - P_sparse = P.sparseView(); - - // NOTE: const std vector cannot be converted to eigen vector - std::vector non_const_q = q; - Eigen::VectorXd eigen_q = - Eigen::Map(non_const_q.data(), non_const_q.size()); - std::vector l_std_vec = l; - Eigen::VectorXd eigen_l = - Eigen::Map(l_std_vec.data(), l_std_vec.size()); - std::vector u_std_vec = u; - Eigen::VectorXd eigen_u = - Eigen::Map(u_std_vec.data(), u_std_vec.size()); - - if (enable_warm_start) { - qp_ptr_->update( - P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); - } else { - qp_ptr_->init( - P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); - } -} - -void ProxQPInterface::updateEpsAbs(const double eps_abs) -{ - settings_.eps_abs = eps_abs; -} - -void ProxQPInterface::updateEpsRel(const double eps_rel) -{ - settings_.eps_rel = eps_rel; -} - -void ProxQPInterface::updateVerbose(const bool is_verbose) -{ - settings_.verbose = is_verbose; -} - -bool ProxQPInterface::isSolved() const -{ - if (qp_ptr_) { - return qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED; - } - return false; -} - -int ProxQPInterface::getIterationNumber() const -{ - if (qp_ptr_) { - return qp_ptr_->results.info.iter; - } - return 0; -} - -std::string ProxQPInterface::getStatus() const -{ - if (qp_ptr_) { - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED) { - return "PROXQP_SOLVED"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_MAX_ITER_REACHED) { - return "PROXQP_MAX_ITER_REACHED"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { - return "PROXQP_PRIMAL_INFEASIBLE"; - } - // if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE) { - // return "PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE"; - // } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_DUAL_INFEASIBLE) { - return "PROXQP_DUAL_INFEASIBLE"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_NOT_RUN) { - return "PROXQP_NOT_RUN"; - } - } - return "None"; -} - -std::vector ProxQPInterface::optimizeImpl() -{ - qp_ptr_->solve(); - - std::vector result; - for (Eigen::Index i = 0; i < qp_ptr_->results.x.size(); ++i) { - result.push_back(qp_ptr_->results.x[i]); - } - return result; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/qp_interface.cpp b/common/autoware_qp_interface/src/qp_interface.cpp deleted file mode 100644 index f01e57772dc4f..0000000000000 --- a/common/autoware_qp_interface/src/qp_interface.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/qp_interface.hpp" - -#include -#include -#include - -namespace autoware::qp_interface -{ -void QPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // check if arguments are valid - std::stringstream ss; - if (P.rows() != P.cols()) { - ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() - << ", P.cols() = " << P.cols(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != static_cast(q.size())) { - ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() - << ", q.size() = " << q.size(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != A.cols()) { - ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() - << ", A.cols() = " << A.cols(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(l.size())) { - ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() - << ", l.size() = " << l.size(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(u.size())) { - ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() - << ", u.size() = " << u.size(); - throw std::invalid_argument(ss.str()); - } - - initializeProblemImpl(P, A, q, l, u); - - variables_num_ = q.size(); - constraints_num_ = l.size(); -} - -std::vector QPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - initializeProblem(P, A, q, l, u); - const auto result = optimizeImpl(); - - return result; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp deleted file mode 100644 index 1f377a1a24535..0000000000000 --- a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -TEST(TestCscMatrixConv, Nominal) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::CSC_Matrix; - - Eigen::MatrixXd rect1(1, 2); - rect1 << 0.0, 1.0; - - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.vals_.size(), size_t(1)); - EXPECT_EQ(rect_m1.vals_[0], 1.0); - ASSERT_EQ(rect_m1.row_idxs_.size(), size_t(1)); - EXPECT_EQ(rect_m1.row_idxs_[0], c_int(0)); - ASSERT_EQ(rect_m1.col_idxs_.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.col_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m1.col_idxs_[1], c_int(0)); - EXPECT_EQ(rect_m1.col_idxs_[2], c_int(1)); - - Eigen::MatrixXd rect2(2, 4); - rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; - - const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.vals_.size(), size_t(4)); - EXPECT_EQ(rect_m2.vals_[0], 1.0); - EXPECT_EQ(rect_m2.vals_[1], 6.0); - EXPECT_EQ(rect_m2.vals_[2], 3.0); - EXPECT_EQ(rect_m2.vals_[3], 7.0); - ASSERT_EQ(rect_m2.row_idxs_.size(), size_t(4)); - EXPECT_EQ(rect_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(rect_m2.row_idxs_[2], c_int(0)); - EXPECT_EQ(rect_m2.row_idxs_[3], c_int(1)); - ASSERT_EQ(rect_m2.col_idxs_.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m2.col_idxs_[1], c_int(1)); - EXPECT_EQ(rect_m2.col_idxs_[2], c_int(2)); - EXPECT_EQ(rect_m2.col_idxs_[3], c_int(4)); - EXPECT_EQ(rect_m2.col_idxs_[4], c_int(4)); - - // Example from http://netlib.org/linalg/html_templates/node92.html - Eigen::MatrixXd square2(6, 6); - square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, - 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; - - const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.vals_.size(), size_t(19)); - EXPECT_EQ(square_m2.vals_[0], 10.0); - EXPECT_EQ(square_m2.vals_[1], 3.0); - EXPECT_EQ(square_m2.vals_[2], 3.0); - EXPECT_EQ(square_m2.vals_[3], 9.0); - EXPECT_EQ(square_m2.vals_[4], 7.0); - EXPECT_EQ(square_m2.vals_[5], 8.0); - EXPECT_EQ(square_m2.vals_[6], 4.0); - EXPECT_EQ(square_m2.vals_[7], 8.0); - EXPECT_EQ(square_m2.vals_[8], 8.0); - EXPECT_EQ(square_m2.vals_[9], 7.0); - EXPECT_EQ(square_m2.vals_[10], 7.0); - EXPECT_EQ(square_m2.vals_[11], 9.0); - EXPECT_EQ(square_m2.vals_[12], -2.0); - EXPECT_EQ(square_m2.vals_[13], 5.0); - EXPECT_EQ(square_m2.vals_[14], 9.0); - EXPECT_EQ(square_m2.vals_[15], 2.0); - EXPECT_EQ(square_m2.vals_[16], 3.0); - EXPECT_EQ(square_m2.vals_[17], 13.0); - EXPECT_EQ(square_m2.vals_[18], -1.0); - ASSERT_EQ(square_m2.row_idxs_.size(), size_t(19)); - EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[2], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[3], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[4], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[5], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[6], c_int(5)); - EXPECT_EQ(square_m2.row_idxs_[7], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[8], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[9], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[10], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[11], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[12], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[13], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[14], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[15], c_int(5)); - EXPECT_EQ(square_m2.row_idxs_[16], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[17], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[18], c_int(5)); - ASSERT_EQ(square_m2.col_idxs_.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[1], c_int(3)); - EXPECT_EQ(square_m2.col_idxs_[2], c_int(7)); - EXPECT_EQ(square_m2.col_idxs_[3], c_int(9)); - EXPECT_EQ(square_m2.col_idxs_[4], c_int(12)); - EXPECT_EQ(square_m2.col_idxs_[5], c_int(16)); - EXPECT_EQ(square_m2.col_idxs_[6], c_int(19)); -} -TEST(TestCscMatrixConv, Trapezoidal) -{ - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd square2(3, 3); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; - rect1 << 0.0, 1.0; - - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.vals_.size(), size_t(3)); - EXPECT_EQ(square_m1.vals_[0], 1.0); - EXPECT_EQ(square_m1.vals_[1], 2.0); - EXPECT_EQ(square_m1.vals_[2], 4.0); - ASSERT_EQ(square_m1.row_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m1.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m1.row_idxs_[1], c_int(0)); - EXPECT_EQ(square_m1.row_idxs_[2], c_int(1)); - ASSERT_EQ(square_m1.col_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m1.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m1.col_idxs_[1], c_int(1)); - EXPECT_EQ(square_m1.col_idxs_[2], c_int(3)); - - const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.vals_.size(), size_t(3)); - EXPECT_EQ(square_m2.vals_[0], 2.0); - EXPECT_EQ(square_m2.vals_[1], 5.0); - EXPECT_EQ(square_m2.vals_[2], 6.0); - ASSERT_EQ(square_m2.row_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[2], c_int(1)); - ASSERT_EQ(square_m2.col_idxs_.size(), size_t(4)); - EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[1], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[2], c_int(2)); - EXPECT_EQ(square_m2.col_idxs_[3], c_int(3)); - - try { - const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); - FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; - } catch (const std::invalid_argument & e) { - EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); - } -} -TEST(TestCscMatrixConv, Print) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - using autoware::qp_interface::printCSCMatrix; - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - rect1 << 0.0, 1.0; - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - printCSCMatrix(square_m1); - printCSCMatrix(rect_m1); -} diff --git a/common/autoware_qp_interface/test/test_osqp_interface.cpp b/common/autoware_qp_interface/test/test_osqp_interface.cpp deleted file mode 100644 index f97cc2888afa0..0000000000000 --- a/common/autoware_qp_interface/test/test_osqp_interface.cpp +++ /dev/null @@ -1,170 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include -#include - -namespace -{ -// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestOsqpInterface, BasicQp) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - - auto check_result = - [](const auto & solution, const std::string & status, const int polish_status) { - EXPECT_EQ(status, "OSQP_SOLVED"); - EXPECT_EQ(polish_status, 1); - - static const auto ep = 1.0e-8; - - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -autoware::qp_interface::OSQP_INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::qp_interface::OSQP_INF}; - - { - // Define problem during optimization - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - // Define problem during initialization - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem - Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); - } - - { - // Define problem during initialization with csc matrix - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - // add warm startup - { - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(true, 4000, 1e-6); // enable warm start - osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - { - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - - osqp.updateCheckTermination(1); - const auto primal_val = solution; - const auto dual_val = osqp.getDualSolution(); - for (size_t i = 0; i < primal_val.size(); ++i) { - std::cerr << primal_val.at(i) << std::endl; - } - osqp.setWarmStart(primal_val, dual_val); - } - - { - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - // NOTE: This should be true, but currently optimize function reset the workspace, which - // disables warm start. - // EXPECT_EQ(osqp.getTakenIter(), 1); - } -} -} // namespace diff --git a/common/autoware_qp_interface/test/test_proxqp_interface.cpp b/common/autoware_qp_interface/test/test_proxqp_interface.cpp deleted file mode 100644 index e47af10c7aabd..0000000000000 --- a/common/autoware_qp_interface/test/test_proxqp_interface.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/proxqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include -#include - -namespace -{ -// Problem taken from -// https://github.com/proxqp/proxqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestProxqpInterface, BasicQp) -{ - auto check_result = [](const auto & solution, const std::string & status) { - EXPECT_EQ(status, "PROXQP_SOLVED"); - - static const auto ep = 1.0e-8; - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -std::numeric_limits::max()}; - const std::vector u = {1.0, 0.7, 0.7, std::numeric_limits::max()}; - - { - // Define problem during optimization - autoware::qp_interface::ProxQPInterface proxqp(false, 4000, 1e-9, 1e-9, false); - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - } - - { - // Define problem during optimization with warm start - autoware::qp_interface::ProxQPInterface proxqp(true, 4000, 1e-9, 1e-9, false); - { - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - EXPECT_NE(proxqp.getIterationNumber(), 1); - } - { - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - EXPECT_EQ(proxqp.getIterationNumber(), 0); - } - } -} -} // namespace From 997146c94c4f5eaa0547206981b1566879c0a266 Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Wed, 15 Jan 2025 10:11:09 +0800 Subject: [PATCH 09/59] feat(autoware_osqp_interface): porting autoware_osqp_interface package to autoware.core (#9890) Signed-off-by: NorahXiong --- common/autoware_osqp_interface/CHANGELOG.rst | 51 -- common/autoware_osqp_interface/CMakeLists.txt | 58 --- .../design/osqp_interface-design.md | 70 --- .../osqp_interface/csc_matrix_conv.hpp | 47 -- .../osqp_interface/osqp_interface.hpp | 194 -------- .../osqp_interface/visibility_control.hpp | 37 -- common/autoware_osqp_interface/package.xml | 29 -- .../src/csc_matrix_conv.cpp | 135 ------ .../src/osqp_interface.cpp | 435 ------------------ .../test/test_csc_matrix_conv.cpp | 181 -------- .../test/test_osqp_interface.cpp | 164 ------- 11 files changed, 1401 deletions(-) delete mode 100644 common/autoware_osqp_interface/CHANGELOG.rst delete mode 100644 common/autoware_osqp_interface/CMakeLists.txt delete mode 100644 common/autoware_osqp_interface/design/osqp_interface-design.md delete mode 100644 common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp delete mode 100644 common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp delete mode 100644 common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp delete mode 100644 common/autoware_osqp_interface/package.xml delete mode 100644 common/autoware_osqp_interface/src/csc_matrix_conv.cpp delete mode 100644 common/autoware_osqp_interface/src/osqp_interface.cpp delete mode 100644 common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp delete mode 100644 common/autoware_osqp_interface/test/test_osqp_interface.cpp diff --git a/common/autoware_osqp_interface/CHANGELOG.rst b/common/autoware_osqp_interface/CHANGELOG.rst deleted file mode 100644 index d400b77f4bf61..0000000000000 --- a/common/autoware_osqp_interface/CHANGELOG.rst +++ /dev/null @@ -1,51 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_osqp_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* fix(autoware_osqp_interface): fix clang-tidy errors (`#9440 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(osqp_interface): added autoware prefix to osqp_interface (`#8958 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_osqp_interface/CMakeLists.txt b/common/autoware_osqp_interface/CMakeLists.txt deleted file mode 100644 index b770af659e52a..0000000000000 --- a/common/autoware_osqp_interface/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_osqp_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) - -# after find_package(osqp_vendor) in ament_auto_find_build_dependencies -find_package(osqp REQUIRED) -get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) -get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) - -set(OSQP_INTERFACE_LIB_SRC - src/csc_matrix_conv.cpp - src/osqp_interface.cpp -) - -set(OSQP_INTERFACE_LIB_HEADERS - include/autoware/osqp_interface/csc_matrix_conv.hpp - include/autoware/osqp_interface/osqp_interface.hpp - include/autoware/osqp_interface/visibility_control.hpp -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - ${OSQP_INTERFACE_LIB_SRC} - ${OSQP_INTERFACE_LIB_HEADERS} -) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) - -target_include_directories(${PROJECT_NAME} - SYSTEM PUBLIC - "${OSQP_INCLUDE_DIR}" - "${EIGEN3_INCLUDE_DIR}" -) - -ament_target_dependencies(${PROJECT_NAME} - Eigen3 - osqp_vendor -) - -# crucial so downstream package builds because autoware_osqp_interface exposes osqp.hpp -ament_export_include_directories("${OSQP_INCLUDE_DIR}") -# crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a -ament_export_libraries(osqp::osqp) - -if(BUILD_TESTING) - set(TEST_SOURCES - test/test_osqp_interface.cpp - test/test_csc_matrix_conv.cpp - ) - set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) - ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/common/autoware_osqp_interface/design/osqp_interface-design.md b/common/autoware_osqp_interface/design/osqp_interface-design.md deleted file mode 100644 index 887a4b4d9af3f..0000000000000 --- a/common/autoware_osqp_interface/design/osqp_interface-design.md +++ /dev/null @@ -1,70 +0,0 @@ -# Interface for the OSQP library - -This is the design document for the `autoware_osqp_interface` package. - -## Purpose / Use cases - - - - -This packages provides a C++ interface for the [OSQP library](https://osqp.org/docs/solver/index.html). - -## Design - - - - -The class `OSQPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into -C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the OSQP workspace dataholder, and runs the optimizer. - -## Inputs / Outputs / API - - - - -The interface can be used in several ways: - -1. Initialize the interface WITHOUT data. Load the problem formulation at the optimization call. - - ```cpp - osqp_interface = OSQPInterface(); - osqp_interface.optimize(P, A, q, l, u); - ``` - -2. Initialize the interface WITH data. - - ```cpp - osqp_interface = OSQPInterface(P, A, q, l, u); - osqp_interface.optimize(); - ``` - -3. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. - - ```cpp - osqp_interface = OSQPInterface(P, A, q, l, u); - osqp_interface.optimize(); - osqp.initializeProblem(P_new, A_new, q_new, l_new, u_new); - osqp_interface.optimize(); - ``` - - The optimization results are returned as a vector by the optimization function. - - ```cpp - std::tuple, std::vector> result = osqp_interface.optimize(); - std::vector param = std::get<0>(result); - double x_0 = param[0]; - double x_1 = param[1]; - ``` - -## References / External links - - - -- OSQP library: - -## Related issues - - diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp deleted file mode 100644 index 8c21553152d70..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ -#define AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ - -#include "autoware/osqp_interface/visibility_control.hpp" -#include "osqp/glob_opts.h" // for 'c_int' type ('long' or 'long long') - -#include - -#include - -namespace autoware::osqp_interface -{ -/// \brief Compressed-Column-Sparse Matrix -struct OSQP_INTERFACE_PUBLIC CSC_Matrix -{ - /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector m_vals; - /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector m_row_idxs; - /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector m_col_idxs; -}; - -/// \brief Calculate CSC matrix from Eigen matrix -OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); -/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix -OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); -/// \brief Print the given CSC matrix to the standard output -OSQP_INTERFACE_PUBLIC void printCSCMatrix(const CSC_Matrix & csc_mat); - -} // namespace autoware::osqp_interface - -#endif // AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp deleted file mode 100644 index ff3013bd61514..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp +++ /dev/null @@ -1,194 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ -#define AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" -#include "autoware/osqp_interface/visibility_control.hpp" -#include "osqp/osqp.h" - -#include -#include - -#include -#include -#include -#include -#include - -namespace autoware::osqp_interface -{ -constexpr c_float INF = 1e30; - -/** - * Implementation of a native C++ interface for the OSQP solver. - * - */ -class OSQP_INTERFACE_PUBLIC OSQPInterface -{ -private: - std::unique_ptr> m_work; - std::unique_ptr m_settings; - std::unique_ptr m_data; - // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo m_latest_work_info; - // Number of parameters to optimize - int64_t m_param_n; - // Flag to check if the current work exists - bool m_work_initialized = false; - // Exitflag - int64_t m_exitflag; - - // Runs the solver on the stored problem. - std::tuple, std::vector, int64_t, int64_t, int64_t> solve(); - - static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; - -public: - /// \brief Constructor without problem formulation - explicit OSQPInterface( - const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); - /// \brief Constructor with problem setup - /// \param P: (n,n) matrix defining relations between parameters. - /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q: (n) vector defining the linear cost of the problem. - /// \param l: (m) vector defining the lower bound problem constraint. - /// \param u: (m) vector defining the upper bound problem constraint. - /// \param eps_abs: Absolute convergence tolerance. - OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); - OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); - ~OSQPInterface(); - - /**************** - * OPTIMIZATION - ****************/ - /// \brief Solves the stored convex quadratic program (QP) problem using the OSQP solver. - // - /// \return The function returns a tuple containing the solution as two float vectors. - /// \return The first element of the tuple contains the 'primal' solution. - /// \return The second element contains the 'lagrange multiplier' solution. - /// \return The third element contains an integer with solver polish status information. - - /// \details How to use: - /// \details 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. - /// \details 2. Initialize the interface and set up the problem. - /// \details osqp_interface = OSQPInterface(P, A, q, l, u, 1e-6); - /// \details 3. Call the optimization function. - /// \details std::tuple, std::vector> result; - /// \details result = osqp_interface.optimize(); - /// \details 4. Access the optimized parameters. - /// \details std::vector param = std::get<0>(result); - /// \details double x_0 = param[0]; - /// \details double x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize(); - - /// \brief Solves convex quadratic programs (QPs) using the OSQP solver. - /// \return The function returns a tuple containing the solution as two float vectors. - /// \return The first element of the tuple contains the 'primal' solution. - /// \return The second element contains the 'lagrange multiplier' solution. - /// \return The third element contains an integer with solver polish status information. - /// \details How to use: - /// \details 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. - /// \details 2. Initialize the interface. - /// \details osqp_interface = OSQPInterface(1e-6); - /// \details 3. Call the optimization function with the problem formulation. - /// \details std::tuple, std::vector> result; - /// \details result = osqp_interface.optimize(P, A, q, l, u, 1e-6); - /// \details 4. Access the optimized parameters. - /// \details std::vector param = std::get<0>(result); - /// \details double x_0 = param[0]; - /// \details double x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - /// \brief Converts the input data and sets up the workspace object. - /// \param P (n,n) matrix defining relations between parameters. - /// \param A (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q (n) vector defining the linear cost of the problem. - /// \param l (m) vector defining the lower bound problem constraint. - /// \param u (m) vector defining the upper bound problem constraint. - int64_t initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - int64_t initializeProblem( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - // Setter functions for warm start - bool setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables); - bool setPrimalVariables(const std::vector & primal_variables); - bool setDualVariables(const std::vector & dual_variables); - - // Updates problem parameters while keeping solution in memory. - // - // Args: - // P_new: (n,n) matrix defining relations between parameters. - // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q_new: (n) vector defining the linear cost of the problem. - // l_new: (m) vector defining the lower bound problem constraint. - // u_new: (m) vector defining the upper bound problem constraint. - void updateP(const Eigen::MatrixXd & P_new); - void updateCscP(const CSC_Matrix & P_csc); - void updateA(const Eigen::MatrixXd & A_new); - void updateCscA(const CSC_Matrix & A_csc); - void updateQ(const std::vector & q_new); - void updateL(const std::vector & l_new); - void updateU(const std::vector & u_new); - void updateBounds(const std::vector & l_new, const std::vector & u_new); - void updateEpsAbs(const double eps_abs); - void updateEpsRel(const double eps_rel); - void updateMaxIter(const int iter); - void updateVerbose(const bool verbose); - void updateRhoInterval(const int rho_interval); - void updateRho(const double rho); - void updateAlpha(const double alpha); - void updateScaling(const int scaling); - void updatePolish(const bool polish); - void updatePolishRefinementIteration(const int polish_refine_iter); - void updateCheckTermination(const int check_termination); - - /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } - /// \brief Get the status message for the latest problem solved - inline std::string getStatusMessage() const - { - return static_cast(m_latest_work_info.status); - } - /// \brief Get the status value for the latest problem solved - inline int64_t getStatus() const { return static_cast(m_latest_work_info.status_val); } - /// \brief Get the status polish for the latest problem solved - inline int64_t getStatusPolish() const - { - return static_cast(m_latest_work_info.status_polish); - } - /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return m_latest_work_info.run_time; } - /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return m_latest_work_info.obj_val; } - /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return m_exitflag; } - - void logUnsolvedStatus(const std::string & prefix_message = "") const; -}; - -} // namespace autoware::osqp_interface - -#endif // AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp deleted file mode 100644 index a6cdaa8a0a74e..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#define OSQP_INTERFACE_PUBLIC __declspec(dllexport) -#define OSQP_INTERFACE_LOCAL -#else // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#define OSQP_INTERFACE_PUBLIC __declspec(dllimport) -#define OSQP_INTERFACE_LOCAL -#endif // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#elif defined(__linux__) -#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default"))) -#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default"))) -#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_osqp_interface/package.xml b/common/autoware_osqp_interface/package.xml deleted file mode 100644 index 92a2afeccc4e0..0000000000000 --- a/common/autoware_osqp_interface/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - autoware_osqp_interface - 0.40.0 - Interface for the OSQP solver - Maxime CLEMENT - Takayuki Murooka - Fumiya Watanabe - Satoshi Ota - Apache 2.0 - - ament_cmake_auto - autoware_cmake - - eigen - osqp_vendor - rclcpp - rclcpp_components - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/common/autoware_osqp_interface/src/csc_matrix_conv.cpp b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp deleted file mode 100644 index c6e1a0a42d938..0000000000000 --- a/common/autoware_osqp_interface/src/csc_matrix_conv.cpp +++ /dev/null @@ -1,135 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" - -#include -#include - -#include -#include -#include - -namespace autoware::osqp_interface -{ -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i < rows; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - if (rows != cols) { - throw std::invalid_argument("Matrix must be square (n, n)"); - } - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - Eigen::Index trap_last_idx = 0; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - trap_last_idx += 1; - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -void printCSCMatrix(const CSC_Matrix & csc_mat) -{ - std::cout << "["; - for (const c_float & val : csc_mat.m_vals) { - std::cout << val << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & row : csc_mat.m_row_idxs) { - std::cout << row << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & col : csc_mat.m_col_idxs) { - std::cout << col << ", "; - } - std::cout << "]\n"; -} - -} // namespace autoware::osqp_interface diff --git a/common/autoware_osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp deleted file mode 100644 index ceeae626222ca..0000000000000 --- a/common/autoware_osqp_interface/src/osqp_interface.cpp +++ /dev/null @@ -1,435 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/osqp_interface/osqp_interface.hpp" - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" -#include "osqp/osqp.h" - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::osqp_interface -{ -OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish) -: m_work{nullptr, OSQPWorkspaceDeleter} -{ - m_settings = std::make_unique(); - m_data = std::make_unique(); - - if (m_settings) { - osqp_set_default_settings(m_settings.get()); - m_settings->alpha = 1.6; // Change alpha parameter - m_settings->eps_rel = 1.0E-4; - m_settings->eps_abs = eps_abs; - m_settings->eps_prim_inf = 1.0E-4; - m_settings->eps_dual_inf = 1.0E-4; - m_settings->warm_start = true; - m_settings->max_iter = 4000; - m_settings->verbose = false; - m_settings->polish = polish; - } - m_exitflag = 0; -} - -OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) -: OSQPInterface(eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) -: OSQPInterface(eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::~OSQPInterface() -{ - if (m_data->P) free(m_data->P); - if (m_data->A) free(m_data->A); -} - -void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept -{ - if (ptr != nullptr) { - osqp_cleanup(ptr); - } -} - -void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) -{ - /* - // Transform 'P' into an 'upper trapezoidal matrix' - Eigen::MatrixXd P_trap = P_new.triangularView(); - // Transform 'P' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix P_sparse = P_trap.sparseView(); - double *P_val_ptr = P_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int P_elem_N = P_sparse.nonZeros(); - */ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); -} - -void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) -{ - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); -} - -void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) -{ - /* - // Transform 'A' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix A_sparse = A_new.sparseView(); - double *A_val_ptr = A_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int A_elem_N = A_sparse.nonZeros(); - */ - CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); - return; -} - -void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) -{ - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); -} - -void OSQPInterface::updateQ(const std::vector & q_new) -{ - std::vector q_tmp(q_new.begin(), q_new.end()); - double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(m_work.get(), q_dyn); -} - -void OSQPInterface::updateL(const std::vector & l_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(m_work.get(), l_dyn); -} - -void OSQPInterface::updateU(const std::vector & u_new) -{ - std::vector u_tmp(u_new.begin(), u_new.end()); - double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(m_work.get(), u_dyn); -} - -void OSQPInterface::updateBounds( - const std::vector & l_new, const std::vector & u_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - std::vector u_tmp(u_new.begin(), u_new.end()); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - osqp_update_bounds(m_work.get(), l_dyn, u_dyn); -} - -void OSQPInterface::updateEpsAbs(const double eps_abs) -{ - m_settings->eps_abs = eps_abs; // for default setting - if (m_work_initialized) { - osqp_update_eps_abs(m_work.get(), eps_abs); // for current work - } -} - -void OSQPInterface::updateEpsRel(const double eps_rel) -{ - m_settings->eps_rel = eps_rel; // for default setting - if (m_work_initialized) { - osqp_update_eps_rel(m_work.get(), eps_rel); // for current work - } -} - -void OSQPInterface::updateMaxIter(const int max_iter) -{ - m_settings->max_iter = max_iter; // for default setting - if (m_work_initialized) { - osqp_update_max_iter(m_work.get(), max_iter); // for current work - } -} - -void OSQPInterface::updateVerbose(const bool is_verbose) -{ - m_settings->verbose = is_verbose; // for default setting - if (m_work_initialized) { - osqp_update_verbose(m_work.get(), is_verbose); // for current work - } -} - -void OSQPInterface::updateRhoInterval(const int rho_interval) -{ - m_settings->adaptive_rho_interval = rho_interval; // for default setting -} - -void OSQPInterface::updateRho(const double rho) -{ - m_settings->rho = rho; - if (m_work_initialized) { - osqp_update_rho(m_work.get(), rho); - } -} - -void OSQPInterface::updateAlpha(const double alpha) -{ - m_settings->alpha = alpha; - if (m_work_initialized) { - osqp_update_alpha(m_work.get(), alpha); - } -} - -void OSQPInterface::updateScaling(const int scaling) -{ - m_settings->scaling = scaling; -} - -void OSQPInterface::updatePolish(const bool polish) -{ - m_settings->polish = polish; - if (m_work_initialized) { - osqp_update_polish(m_work.get(), polish); - } -} - -void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) -{ - if (polish_refine_iter < 0) { - std::cerr << "Polish refinement iterations must be positive" << std::endl; - return; - } - - m_settings->polish_refine_iter = polish_refine_iter; - if (m_work_initialized) { - osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); - } -} - -void OSQPInterface::updateCheckTermination(const int check_termination) -{ - if (check_termination < 0) { - std::cerr << "Check termination must be positive" << std::endl; - return; - } - - m_settings->check_termination = check_termination; - if (m_work_initialized) { - osqp_update_check_termination(m_work.get(), check_termination); - } -} - -bool OSQPInterface::setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables) -{ - return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); -} - -bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) -{ - if (!m_work_initialized) { - return false; - } - - const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); - if (result != 0) { - std::cerr << "Failed to set primal variables for warm start" << std::endl; - return false; - } - - return true; -} - -bool OSQPInterface::setDualVariables(const std::vector & dual_variables) -{ - if (!m_work_initialized) { - return false; - } - - const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); - if (result != 0) { - std::cerr << "Failed to set dual variables for warm start" << std::endl; - return false; - } - - return true; -} - -int64_t OSQPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // check if arguments are valid - std::stringstream ss; - if (P.rows() != P.cols()) { - ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() - << ", P.cols() = " << P.cols(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != static_cast(q.size())) { - ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() - << ", q.size() = " << q.size(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != A.cols()) { - ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() - << ", A.cols() = " << A.cols(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(l.size())) { - ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() - << ", l.size() = " << l.size(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(u.size())) { - ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() - << ", u.size() = " << u.size(); - throw std::invalid_argument(ss.str()); - } - - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - return initializeProblem(P_csc, A_csc, q, l, u); -} - -int64_t OSQPInterface::initializeProblem( - CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector u_tmp(u.begin(), u.end()); - double * q_dyn = q_tmp.data(); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - - /********************** - * OBJECTIVE FUNCTION - **********************/ - m_param_n = static_cast(q.size()); - m_data->m = static_cast(l.size()); - - /***************** - * POPULATE DATA - *****************/ - m_data->n = m_param_n; - if (m_data->P) free(m_data->P); - m_data->P = csc_matrix( - m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), - P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); - m_data->q = q_dyn; - if (m_data->A) free(m_data->A); - m_data->A = csc_matrix( - m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), - A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); - m_data->l = l_dyn; - m_data->u = u_dyn; - - // Setup workspace - OSQPWorkspace * workspace; - m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); - m_work.reset(workspace); - m_work_initialized = true; - - return m_exitflag; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::solve() -{ - // Solve Problem - osqp_solve(m_work.get()); - - /******************** - * EXTRACT SOLUTION - ********************/ - double * sol_x = m_work->solution->x; - double * sol_y = m_work->solution->y; - std::vector sol_primal(sol_x, sol_x + m_param_n); - std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); - - int64_t status_polish = m_work->info->status_polish; - int64_t status_solution = m_work->info->status_val; - int64_t status_iteration = m_work->info->iter; - - // Result tuple - std::tuple, std::vector, int64_t, int64_t, int64_t> result = - std::make_tuple( - sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration); - - m_latest_work_info = *(m_work->info); - - return result; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::optimize() -{ - // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); - return result; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // Allocate memory for problem - initializeProblem(P, A, q, l, u); - - // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); - - m_work.reset(); - m_work_initialized = false; - - return result; -} - -void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const -{ - const int status = getStatus(); - if (status == 1) { - // No need to log since optimization was solved. - return; - } - - // create message - std::string output_message = ""; - if (prefix_message != "") { - output_message = prefix_message + " "; - } - - const auto status_message = getStatusMessage(); - output_message += "Optimization failed due to " + status_message; - - // log with warning - RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), "%s", output_message.c_str()); -} -} // namespace autoware::osqp_interface diff --git a/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp deleted file mode 100644 index a63f979a966bf..0000000000000 --- a/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -TEST(TestCscMatrixConv, Nominal) -{ - using autoware::osqp_interface::calCSCMatrix; - using autoware::osqp_interface::CSC_Matrix; - - Eigen::MatrixXd rect1(1, 2); - rect1 << 0.0, 1.0; - - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_vals[0], 1.0); - ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); - ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); - - Eigen::MatrixXd rect2(2, 4); - rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; - - const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_vals[0], 1.0); - EXPECT_EQ(rect_m2.m_vals[1], 6.0); - EXPECT_EQ(rect_m2.m_vals[2], 3.0); - EXPECT_EQ(rect_m2.m_vals[3], 7.0); - ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); - ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); - EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); - - // Example from http://netlib.org/linalg/html_templates/node92.html - Eigen::MatrixXd square2(6, 6); - square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, - 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; - - const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); - EXPECT_EQ(square_m2.m_vals[0], 10.0); - EXPECT_EQ(square_m2.m_vals[1], 3.0); - EXPECT_EQ(square_m2.m_vals[2], 3.0); - EXPECT_EQ(square_m2.m_vals[3], 9.0); - EXPECT_EQ(square_m2.m_vals[4], 7.0); - EXPECT_EQ(square_m2.m_vals[5], 8.0); - EXPECT_EQ(square_m2.m_vals[6], 4.0); - EXPECT_EQ(square_m2.m_vals[7], 8.0); - EXPECT_EQ(square_m2.m_vals[8], 8.0); - EXPECT_EQ(square_m2.m_vals[9], 7.0); - EXPECT_EQ(square_m2.m_vals[10], 7.0); - EXPECT_EQ(square_m2.m_vals[11], 9.0); - EXPECT_EQ(square_m2.m_vals[12], -2.0); - EXPECT_EQ(square_m2.m_vals[13], 5.0); - EXPECT_EQ(square_m2.m_vals[14], 9.0); - EXPECT_EQ(square_m2.m_vals[15], 2.0); - EXPECT_EQ(square_m2.m_vals[16], 3.0); - EXPECT_EQ(square_m2.m_vals[17], 13.0); - EXPECT_EQ(square_m2.m_vals[18], -1.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); - EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); - EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); - EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); -} -TEST(TestCscMatrixConv, Trapezoidal) -{ - using autoware::osqp_interface::calCSCMatrixTrapezoidal; - using autoware::osqp_interface::CSC_Matrix; - - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd square2(3, 3); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; - rect1 << 0.0, 1.0; - - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m1.m_vals[0], 1.0); - EXPECT_EQ(square_m1.m_vals[1], 2.0); - EXPECT_EQ(square_m1.m_vals[2], 4.0); - ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); - EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); - - const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m2.m_vals[0], 2.0); - EXPECT_EQ(square_m2.m_vals[1], 5.0); - EXPECT_EQ(square_m2.m_vals[2], 6.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); - - try { - const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); - FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; - } catch (const std::invalid_argument & e) { - EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); - } -} -TEST(TestCscMatrixConv, Print) -{ - using autoware::osqp_interface::calCSCMatrix; - using autoware::osqp_interface::calCSCMatrixTrapezoidal; - using autoware::osqp_interface::CSC_Matrix; - using autoware::osqp_interface::printCSCMatrix; - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - rect1 << 0.0, 1.0; - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - printCSCMatrix(square_m1); - printCSCMatrix(rect_m1); -} diff --git a/common/autoware_osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp deleted file mode 100644 index f65b07e6d792d..0000000000000 --- a/common/autoware_osqp_interface/test/test_osqp_interface.cpp +++ /dev/null @@ -1,164 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/osqp_interface/osqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -namespace -{ -// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestOsqpInterface, BasicQp) -{ - using autoware::osqp_interface::calCSCMatrix; - using autoware::osqp_interface::calCSCMatrixTrapezoidal; - using autoware::osqp_interface::CSC_Matrix; - - auto check_result = - [](const std::tuple, std::vector, int, int, int> & result) { - EXPECT_EQ(std::get<2>(result), 1); // polish succeeded - EXPECT_EQ(std::get<3>(result), 1); // solution succeeded - - static const auto ep = 1.0e-8; - - const auto prime_val = std::get<0>(result); - ASSERT_EQ(prime_val.size(), size_t(2)); - EXPECT_NEAR(prime_val[0], 0.3, ep); - EXPECT_NEAR(prime_val[1], 0.7, ep); - - const auto dual_val = std::get<1>(result); - ASSERT_EQ(dual_val.size(), size_t(4)); - EXPECT_NEAR(dual_val[0], -2.9, ep); - EXPECT_NEAR(dual_val[1], 0.0, ep); - EXPECT_NEAR(dual_val[2], 0.2, ep); - EXPECT_NEAR(dual_val[3], 0.0, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -autoware::osqp_interface::INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::osqp_interface::INF}; - - { - // Define problem during optimization - autoware::osqp_interface::OSQPInterface osqp; - std::tuple, std::vector, int, int, int> result = - osqp.optimize(P, A, q, l, u); - check_result(result); - } - - { - // Define problem during initialization - autoware::osqp_interface::OSQPInterface osqp(P, A, q, l, u, 1e-6); - std::tuple, std::vector, int, int, int> result = osqp.optimize(); - check_result(result); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem - Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::osqp_interface::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); - osqp.optimize(); - - // Redefine problem before optimization - osqp.initializeProblem(P, A, q, l, u); - result = osqp.optimize(); - check_result(result); - } - - { - // Define problem during initialization with csc matrix - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - autoware::osqp_interface::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); - std::tuple, std::vector, int, int, int> result = osqp.optimize(); - check_result(result); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); - osqp.optimize(); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - osqp.initializeProblem(P_csc, A_csc, q, l, u); - result = osqp.optimize(); - check_result(result); - } - - // add warm startup - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); - osqp.optimize(); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - osqp.initializeProblem(P_csc, A_csc, q, l, u); - result = osqp.optimize(); - check_result(result); - - osqp.updateCheckTermination(1); - const auto primal_val = std::get<0>(result); - const auto dual_val = std::get<1>(result); - for (size_t i = 0; i < primal_val.size(); ++i) { - std::cerr << primal_val.at(i) << std::endl; - } - osqp.setWarmStart(primal_val, dual_val); - result = osqp.optimize(); - check_result(result); - EXPECT_EQ(osqp.getTakenIter(), 1); - } -} -} // namespace From a6b5f3c6992e6978eec4c110d1956df99c0219ca Mon Sep 17 00:00:00 2001 From: cyn-liu <104069308+cyn-liu@users.noreply.github.com> Date: Wed, 15 Jan 2025 10:17:04 +0800 Subject: [PATCH 10/59] feat(autoware_kalman_filter): move autoware_kalman_filter to autowar.core (#9872) feat(autoware_kalman_filter): move autoware_kalman_filter to autoware.core Signed-off-by: liu cui --- common/autoware_kalman_filter/CHANGELOG.rst | 55 ----- common/autoware_kalman_filter/CMakeLists.txt | 29 --- common/autoware_kalman_filter/README.md | 9 - .../autoware/kalman_filter/kalman_filter.hpp | 214 ------------------ .../time_delay_kalman_filter.hpp | 89 -------- common/autoware_kalman_filter/package.xml | 28 --- .../src/kalman_filter.cpp | 161 ------------- .../src/time_delay_kalman_filter.cpp | 109 --------- .../test/test_kalman_filter.cpp | 96 -------- .../test/test_time_delay_kalman_filter.cpp | 123 ---------- 10 files changed, 913 deletions(-) delete mode 100644 common/autoware_kalman_filter/CHANGELOG.rst delete mode 100644 common/autoware_kalman_filter/CMakeLists.txt delete mode 100644 common/autoware_kalman_filter/README.md delete mode 100644 common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp delete mode 100644 common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp delete mode 100644 common/autoware_kalman_filter/package.xml delete mode 100644 common/autoware_kalman_filter/src/kalman_filter.cpp delete mode 100644 common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp delete mode 100644 common/autoware_kalman_filter/test/test_kalman_filter.cpp delete mode 100644 common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp diff --git a/common/autoware_kalman_filter/CHANGELOG.rst b/common/autoware_kalman_filter/CHANGELOG.rst deleted file mode 100644 index a7e68f8843e74..0000000000000 --- a/common/autoware_kalman_filter/CHANGELOG.rst +++ /dev/null @@ -1,55 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_kalman_filter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(kalman_filter): prefix package and namespace with autoware (`#7787 `_) - * refactor(kalman_filter): prefix package and namespace with autoware - * move headers to include/autoware/ - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_kalman_filter/CMakeLists.txt b/common/autoware_kalman_filter/CMakeLists.txt deleted file mode 100644 index 076d2d3cad4e8..0000000000000 --- a/common/autoware_kalman_filter/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_kalman_filter) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) - -include_directories( - SYSTEM - ${EIGEN3_INCLUDE_DIR} -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/kalman_filter.cpp - src/time_delay_kalman_filter.cpp - include/autoware/kalman_filter/kalman_filter.hpp - include/autoware/kalman_filter/time_delay_kalman_filter.hpp -) - -if(BUILD_TESTING) - file(GLOB_RECURSE test_files test/*.cpp) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) -endif() - -ament_auto_package() diff --git a/common/autoware_kalman_filter/README.md b/common/autoware_kalman_filter/README.md deleted file mode 100644 index 7c0feb9c2a61a..0000000000000 --- a/common/autoware_kalman_filter/README.md +++ /dev/null @@ -1,9 +0,0 @@ -# kalman_filter - -## Purpose - -This common package contains the kalman filter with time delay and the calculation of the kalman filter. - -## Assumptions / Known limits - -TBD. diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp deleted file mode 100644 index 74db04f6e838b..0000000000000 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ -#define AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ - -#include -#include - -namespace autoware::kalman_filter -{ - -/** - * @file kalman_filter.h - * @brief kalman filter class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -class KalmanFilter -{ -public: - /** - * @brief No initialization constructor. - */ - KalmanFilter(); - - /** - * @brief constructor with initialization - * @param x initial state - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param C coefficient matrix of x for measurement model - * @param Q covariance matrix for process model - * @param R covariance matrix for measurement model - * @param P initial covariance of estimated state - */ - KalmanFilter( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P); - - /** - * @brief destructor - */ - ~KalmanFilter(); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param C coefficient matrix of x for measurement model - * @param Q covariance matrix for process model - * @param R covariance matrix for measurement model - * @param P initial covariance of estimated state - */ - bool init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param P initial covariance of estimated state - */ - bool init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0); - - /** - * @brief set A of process model - * @param A coefficient matrix of x for process model - */ - void setA(const Eigen::MatrixXd & A); - - /** - * @brief set B of process model - * @param B coefficient matrix of u for process model - */ - void setB(const Eigen::MatrixXd & B); - - /** - * @brief set C of measurement model - * @param C coefficient matrix of x for measurement model - */ - void setC(const Eigen::MatrixXd & C); - - /** - * @brief set covariance matrix Q for process model - * @param Q covariance matrix for process model - */ - void setQ(const Eigen::MatrixXd & Q); - - /** - * @brief set covariance matrix R for measurement model - * @param R covariance matrix for measurement model - */ - void setR(const Eigen::MatrixXd & R); - - /** - * @brief get current kalman filter state - * @param x kalman filter state - */ - void getX(Eigen::MatrixXd & x) const; - - /** - * @brief get current kalman filter covariance - * @param P kalman filter covariance - */ - void getP(Eigen::MatrixXd & P) const; - - /** - * @brief get component of current kalman filter state - * @param i index of kalman filter state - * @return value of i's component of the kalman filter state x[i] - */ - double getXelement(unsigned int i) const; - - /** - * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. - * This is mainly for EKF with variable matrix. - * @param u input for model - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param Q covariance matrix for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict( - const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is - * mainly for EKF with variable matrix. - * @param x_next predicted state - * @param A coefficient matrix of x for process model - * @param Q covariance matrix for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is - * mainly for EKF with variable matrix. - * @param x_next predicted state - * @param A coefficient matrix of x for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A); - - /** - * @brief calculate kalman filter state by prediction model with A, B and Q being class member - * variables. - * @param u input for the model - * @return bool to check matrix operations are being performed properly - */ - bool predict(const Eigen::MatrixXd & u); - - /** - * @brief calculate kalman filter state by measurement model with y_pred, C and R matrix. This is - * mainly for EKF with variable matrix. - * @param y measured values - * @param y output values expected from measurement model - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @return bool to check matrix operations are being performed properly - */ - bool update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, - const Eigen::MatrixXd & R); - - /** - * @brief calculate kalman filter state by measurement model with C and R matrix. This is mainly - * for EKF with variable matrix. - * @param y measured values - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @return bool to check matrix operations are being performed properly - */ - bool update(const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R); - - /** - * @brief calculate kalman filter state by measurement model with C and R being class member - * variables. - * @param y measured values - * @return bool to check matrix operations are being performed properly - */ - bool update(const Eigen::MatrixXd & y); - -protected: - Eigen::MatrixXd x_; //!< @brief current estimated state - Eigen::MatrixXd - A_; //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd - B_; //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd C_; //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k] - Eigen::MatrixXd Q_; //!< @brief covariance matrix for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd R_; //!< @brief covariance matrix for measurement model y[k] = C * x[k] - Eigen::MatrixXd P_; //!< @brief covariance of estimated state -}; -} // namespace autoware::kalman_filter -#endif // AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp deleted file mode 100644 index 80375b7579e62..0000000000000 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ -#define AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ - -#include "autoware/kalman_filter/kalman_filter.hpp" - -#include -#include - -#include - -namespace autoware::kalman_filter -{ -/** - * @file time_delay_kalman_filter.h - * @brief kalman filter with delayed measurement class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -class TimeDelayKalmanFilter : public KalmanFilter -{ -public: - /** - * @brief No initialization constructor. - */ - TimeDelayKalmanFilter(); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param P0 initial covariance of estimated state - * @param max_delay_step Maximum number of delay steps, which determines the dimension of the - * extended kalman filter - */ - void init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P, const int max_delay_step); - - /** - * @brief get latest time estimated state - */ - Eigen::MatrixXd getLatestX() const; - - /** - * @brief get latest time estimation covariance - */ - Eigen::MatrixXd getLatestP() const; - - /** - * @brief calculate kalman filter covariance by precision model with time delay. This is mainly - * for EKF of nonlinear process model. - * @param x_next predicted state by prediction model - * @param A coefficient matrix of x for process model - * @param Q covariance matrix for process model - */ - bool predictWithDelay( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance by measurement model with time delay. This is mainly - * for EKF of nonlinear process model. - * @param y measured values - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @param delay_step measurement delay - */ - bool updateWithDelay( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, - const int delay_step); - -private: - int max_delay_step_; //!< @brief maximum number of delay steps - int dim_x_; //!< @brief dimension of latest state - int dim_x_ex_; //!< @brief dimension of extended state with dime delay -}; -} // namespace autoware::kalman_filter -#endif // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/package.xml b/common/autoware_kalman_filter/package.xml deleted file mode 100644 index e51bb06e9de43..0000000000000 --- a/common/autoware_kalman_filter/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - autoware_kalman_filter - 0.40.0 - The kalman filter package - Yukihiro Saito - Takeshi Ishita - Koji Minoda - Apache License 2.0 - - Takamasa Horibe - - ament_cmake_auto - autoware_cmake - - eigen - eigen3_cmake_module - - ament_cmake_cppcheck - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp deleted file mode 100644 index bbd963675f9e2..0000000000000 --- a/common/autoware_kalman_filter/src/kalman_filter.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/kalman_filter.hpp" - -namespace autoware::kalman_filter -{ -KalmanFilter::KalmanFilter() -{ -} -KalmanFilter::KalmanFilter( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P) -{ - init(x, A, B, C, Q, R, P); -} -KalmanFilter::~KalmanFilter() -{ -} -bool KalmanFilter::init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P) -{ - if ( - x.cols() == 0 || x.rows() == 0 || A.cols() == 0 || A.rows() == 0 || B.cols() == 0 || - B.rows() == 0 || C.cols() == 0 || C.rows() == 0 || Q.cols() == 0 || Q.rows() == 0 || - R.cols() == 0 || R.rows() == 0 || P.cols() == 0 || P.rows() == 0) { - return false; - } - x_ = x; - A_ = A; - B_ = B; - C_ = C; - Q_ = Q; - R_ = R; - P_ = P; - return true; -} -bool KalmanFilter::init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0) -{ - if (x.cols() == 0 || x.rows() == 0 || P0.cols() == 0 || P0.rows() == 0) { - return false; - } - x_ = x; - P_ = P0; - return true; -} - -void KalmanFilter::setA(const Eigen::MatrixXd & A) -{ - A_ = A; -} -void KalmanFilter::setB(const Eigen::MatrixXd & B) -{ - B_ = B; -} -void KalmanFilter::setC(const Eigen::MatrixXd & C) -{ - C_ = C; -} -void KalmanFilter::setQ(const Eigen::MatrixXd & Q) -{ - Q_ = Q; -} -void KalmanFilter::setR(const Eigen::MatrixXd & R) -{ - R_ = R; -} -void KalmanFilter::getX(Eigen::MatrixXd & x) const -{ - x = x_; -} -void KalmanFilter::getP(Eigen::MatrixXd & P) const -{ - P = P_; -} -double KalmanFilter::getXelement(unsigned int i) const -{ - return x_(i); -} - -bool KalmanFilter::predict( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) -{ - if ( - x_.rows() != x_next.rows() || A.cols() != P_.rows() || Q.cols() != Q.rows() || - A.rows() != Q.cols()) { - return false; - } - x_ = x_next; - P_ = A * P_ * A.transpose() + Q; - return true; -} -bool KalmanFilter::predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A) -{ - return predict(x_next, A, Q_); -} - -bool KalmanFilter::predict( - const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & Q) -{ - if (A.cols() != x_.rows() || B.cols() != u.rows()) { - return false; - } - const Eigen::MatrixXd x_next = A * x_ + B * u; - return predict(x_next, A, Q); -} -bool KalmanFilter::predict(const Eigen::MatrixXd & u) -{ - return predict(u, A_, B_, Q_); -} - -bool KalmanFilter::update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, - const Eigen::MatrixXd & R) -{ - if ( - P_.cols() != C.cols() || R.rows() != R.cols() || R.rows() != C.rows() || - y.rows() != y_pred.rows() || y.rows() != C.rows()) { - return false; - } - const Eigen::MatrixXd PCT = P_ * C.transpose(); - const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse()); - - if (isnan(K.array()).any() || isinf(K.array()).any()) { - return false; - } - - x_ = x_ + K * (y - y_pred); - P_ = P_ - K * (C * P_); - return true; -} - -bool KalmanFilter::update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R) -{ - if (C.cols() != x_.rows()) { - return false; - } - const Eigen::MatrixXd y_pred = C * x_; - return update(y, y_pred, C, R); -} -bool KalmanFilter::update(const Eigen::MatrixXd & y) -{ - return update(y, C_, R_); -} -} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp deleted file mode 100644 index 4d1dd8f33b7a6..0000000000000 --- a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/time_delay_kalman_filter.hpp" - -#include - -namespace autoware::kalman_filter -{ -TimeDelayKalmanFilter::TimeDelayKalmanFilter() -{ -} - -void TimeDelayKalmanFilter::init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step) -{ - max_delay_step_ = max_delay_step; - dim_x_ = x.rows(); - dim_x_ex_ = dim_x_ * max_delay_step; - - x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1); - P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); - - for (int i = 0; i < max_delay_step_; ++i) { - x_.block(i * dim_x_, 0, dim_x_, 1) = x; - P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0; - } -} - -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const -{ - return x_.block(0, 0, dim_x_, 1); -} - -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const -{ - return P_.block(0, 0, dim_x_, dim_x_); -} - -bool TimeDelayKalmanFilter::predictWithDelay( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) -{ - /* - * time delay model: - * - * [A 0 0] [P11 P12 P13] [Q 0 0] - * A = [I 0 0], P = [P21 P22 P23], Q = [0 0 0] - * [0 I 0] [P31 P32 P33] [0 0 0] - * - * covariance calculation in prediction : P = A * P * A' + Q - * - * [A*P11*A'*+Q A*P11 A*P12] - * P = [ P11*A' P11 P12] - * [ P21*A' P21 P22] - */ - - const int d_dim_x = dim_x_ex_ - dim_x_; - - /* slide states in the time direction */ - Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, 1); - x_tmp.block(0, 0, dim_x_, 1) = x_next; - x_tmp.block(dim_x_, 0, d_dim_x, 1) = x_.block(0, 0, d_dim_x, 1); - x_ = x_tmp; - - /* update P with delayed measurement A matrix structure */ - Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); - P_tmp.block(0, 0, dim_x_, dim_x_) = A * P_.block(0, 0, dim_x_, dim_x_) * A.transpose() + Q; - P_tmp.block(0, dim_x_, dim_x_, d_dim_x) = A * P_.block(0, 0, dim_x_, d_dim_x); - P_tmp.block(dim_x_, 0, d_dim_x, dim_x_) = P_.block(0, 0, d_dim_x, dim_x_) * A.transpose(); - P_tmp.block(dim_x_, dim_x_, d_dim_x, d_dim_x) = P_.block(0, 0, d_dim_x, d_dim_x); - P_ = P_tmp; - - return true; -} - -bool TimeDelayKalmanFilter::updateWithDelay( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, - const int delay_step) -{ - if (delay_step >= max_delay_step_) { - std::cerr << "delay step is larger than max_delay_step. ignore update." << std::endl; - return false; - } - - const int dim_y = y.rows(); - - /* set measurement matrix */ - Eigen::MatrixXd C_ex = Eigen::MatrixXd::Zero(dim_y, dim_x_ex_); - C_ex.block(0, dim_x_ * delay_step, dim_y, dim_x_) = C; - - /* update */ - if (!update(y, C_ex, R)) { - return false; - } - - return true; -} -} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/test/test_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_kalman_filter.cpp deleted file mode 100644 index 34e23ef9d06e2..0000000000000 --- a/common/autoware_kalman_filter/test/test_kalman_filter.cpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/kalman_filter.hpp" - -#include - -using autoware::kalman_filter::KalmanFilter; - -TEST(kalman_filter, kf) -{ - KalmanFilter kf_; - - Eigen::MatrixXd x_t(2, 1); - x_t << 1, 2; - - Eigen::MatrixXd P_t(2, 2); - P_t << 1, 0, 0, 1; - - Eigen::MatrixXd Q_t(2, 2); - Q_t << 0.01, 0, 0, 0.01; - - Eigen::MatrixXd R_t(2, 2); - R_t << 0.09, 0, 0, 0.09; - - Eigen::MatrixXd C_t(2, 2); - C_t << 1, 0, 0, 1; - - Eigen::MatrixXd A_t(2, 2); - A_t << 1, 0, 0, 1; - - Eigen::MatrixXd B_t(2, 2); - B_t << 1, 0, 0, 1; - - // Initialize the filter and check if initialization was successful - EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t)); - - // Perform prediction - Eigen::MatrixXd u_t(2, 1); - u_t << 0.1, 0.1; - EXPECT_TRUE(kf_.predict(u_t)); - - // Check the updated state and covariance matrix - Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t; - Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t; - - Eigen::MatrixXd x_predict; - kf_.getX(x_predict); - Eigen::MatrixXd P_predict; - kf_.getP(P_predict); - - EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); - EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); - - // Perform update - Eigen::MatrixXd y_t(2, 1); - y_t << 1.05, 2.05; - EXPECT_TRUE(kf_.update(y_t)); - - // Check the updated state and covariance matrix - const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose(); - const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse()); - const Eigen::MatrixXd y_pred = C_t * x_predict_expected; - Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred); - Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected); - - Eigen::MatrixXd x_update; - kf_.getX(x_update); - Eigen::MatrixXd P_update; - kf_.getP(P_update); - - EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); - EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); - EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); - EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); -} - -int main(int argc, char * argv[]) -{ - testing::InitGoogleTest(&argc, argv); - bool result = RUN_ALL_TESTS(); - return result; -} diff --git a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp deleted file mode 100644 index 50c22fae123bc..0000000000000 --- a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/time_delay_kalman_filter.hpp" - -#include - -using autoware::kalman_filter::TimeDelayKalmanFilter; - -TEST(time_delay_kalman_filter, td_kf) -{ - TimeDelayKalmanFilter td_kf_; - - Eigen::MatrixXd x_t(3, 1); - x_t << 1.0, 2.0, 3.0; - Eigen::MatrixXd P_t(3, 3); - P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3; - const int max_delay_step = 5; - const int dim_x = x_t.rows(); - const int dim_x_ex = dim_x * max_delay_step; - // Initialize the filter - td_kf_.init(x_t, P_t, max_delay_step); - - // Check if initialization was successful - Eigen::MatrixXd x_init = td_kf_.getLatestX(); - Eigen::MatrixXd P_init = td_kf_.getLatestP(); - Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1); - Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); - for (int i = 0; i < max_delay_step; ++i) { - x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t; - P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t; - } - - EXPECT_EQ(x_init.rows(), 3); - EXPECT_EQ(x_init.cols(), 1); - EXPECT_EQ(P_init.rows(), 3); - EXPECT_EQ(P_init.cols(), 3); - EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5); - EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5); - EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5); - EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5); - EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5); - EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5); - - // Define prediction parameters - Eigen::MatrixXd A_t(3, 3); - A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0; - Eigen::MatrixXd Q_t(3, 3); - Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03; - Eigen::MatrixXd x_next(3, 1); - x_next << 2.0, 4.0, 6.0; - - // Perform prediction - EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t)); - - // Check the prediction state and covariance matrix - Eigen::MatrixXd x_predict = td_kf_.getLatestX(); - Eigen::MatrixXd P_predict = td_kf_.getLatestP(); - Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1); - x_tmp.block(0, 0, dim_x, 1) = A_t * x_t; - x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1); - x_ex_t = x_tmp; - Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1); - Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); - P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t; - P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) = - A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x); - P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) = - P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose(); - P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) = - P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x); - P_ex_t = P_tmp; - Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x); - EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); - EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5); - EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); - EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5); - - // Define update parameters - Eigen::MatrixXd C_t(3, 3); - C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5; - Eigen::MatrixXd R_t(3, 3); - R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003; - Eigen::MatrixXd y_t(3, 1); - y_t << 1.05, 2.05, 3.05; - const int delay_step = 2; // Choose an appropriate delay step - const int dim_y = y_t.rows(); - - // Perform update - EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step)); - - // Check the updated state and covariance matrix - Eigen::MatrixXd x_update = td_kf_.getLatestX(); - Eigen::MatrixXd P_update = td_kf_.getLatestP(); - - Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex); - const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose(); - const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse()); - const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t; - x_ex_t = x_ex_t + K_t * (y_t - y_pred); - P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t); - Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1); - Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x); - EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); - EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); - EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5); - EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); - EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); - EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5); -} From 7f496888b6ef10eb5ee9e9897505220d92415164 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 11:18:06 +0900 Subject: [PATCH 11/59] feat(autoware_costmap_generator): tier4_debug_msgs changed to autoware_internal-debug_msgs in autoware_costmap_generator (#9901) feat: tier4_debug_msgs changed to autoware_internal-debug_msgs in files planning/autoware_costmap_generator Signed-off-by: vish0012 --- .../autoware/costmap_generator/costmap_generator.hpp | 5 +++-- .../autoware_costmap_generator/src/costmap_generator.cpp | 7 ++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp index 4fed5bcb6cff3..b6749117a6177 100644 --- a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp @@ -58,10 +58,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -99,7 +99,8 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_costmap_; rclcpp::Publisher::SharedPtr pub_occupancy_grid_; rclcpp::Publisher::SharedPtr pub_processing_time_; - rclcpp::Publisher::SharedPtr pub_processing_time_ms_; + rclcpp::Publisher::SharedPtr + pub_processing_time_ms_; rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; autoware::universe_utils::InterProcessPollingSubscriber diff --git a/planning/autoware_costmap_generator/src/costmap_generator.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp index 081a6ead91cda..70b8051bcdc12 100644 --- a/planning/autoware_costmap_generator/src/costmap_generator.cpp +++ b/planning/autoware_costmap_generator/src/costmap_generator.cpp @@ -176,7 +176,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) create_publisher("processing_time", 1); time_keeper_ = std::make_shared(pub_processing_time_); pub_processing_time_ms_ = - this->create_publisher("~/debug/processing_time_ms", 1); + this->create_publisher( + "~/debug/processing_time_ms", 1); // Timer const auto period_ns = rclcpp::Rate(param_->update_rate).period(); @@ -269,7 +270,7 @@ void CostmapGenerator::onTimer() if (!isActive()) { // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_ms_->publish(processing_time_msg); @@ -485,7 +486,7 @@ void CostmapGenerator::publishCostmap( pub_costmap_->publish(*out_gridmap_msg); // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_ms_->publish(processing_time_msg); From 9a2ebcc9f1f55f3aa28ad46a6d0b825f52e45cea Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 11:21:18 +0900 Subject: [PATCH 12/59] feat(autoware_surround_obstacle_checker): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_surround_obstacle_checker (#9915) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_surround_obstacle_checker Signed-off-by: vish0012 --- planning/autoware_surround_obstacle_checker/src/node.cpp | 6 +++--- planning/autoware_surround_obstacle_checker/src/node.hpp | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 0adca312ca733..b3881a8f4fbd9 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -81,8 +81,8 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local()); pub_velocity_limit_ = this->create_publisher( "~/output/max_velocity", rclcpp::QoS{1}.transient_local()); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -231,7 +231,7 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->pushPose(odometry_ptr_->pose.pose, PoseType::NoStart); } - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index f84ad3a8f5987..b6d6bffefed1a 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -24,12 +24,12 @@ #include #include +#include #include #include #include #include #include -#include #include #include #include @@ -106,7 +106,8 @@ class SurroundObstacleCheckerNode : public rclcpp::Node this, "~/input/objects"}; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; From 2028af955d261cc3dfd4836f7501b60a5e030a59 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 11:23:07 +0900 Subject: [PATCH 13/59] feat(autoware_freespace_planner): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_freespace_planner (#9903) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in flies planning/autoware_freespace_planner Signed-off-by: vish0012 --- .../autoware/freespace_planner/freespace_planner_node.hpp | 5 +++-- .../autoware_freespace_planner/freespace_planner_node.cpp | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index 1f5a52ca040a0..bd431372b70dc 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -39,13 +39,13 @@ #include #include +#include #include #include #include #include #include #include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -112,7 +112,8 @@ class FreespacePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pose_array_pub_; rclcpp::Publisher::SharedPtr debug_partial_pose_array_pub_; rclcpp::Publisher::SharedPtr parking_state_pub_; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; rclcpp::Subscription::SharedPtr route_sub_; diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index a937114e653c6..2749408bedb7b 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -95,8 +95,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti debug_pose_array_pub_ = create_publisher("~/debug/pose_array", qos); debug_partial_pose_array_pub_ = create_publisher("~/debug/partial_pose_array", qos); parking_state_pub_ = create_publisher("is_completed", qos); - processing_time_pub_ = - create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = create_publisher( + "~/debug/processing_time_ms", 1); } // TF @@ -362,7 +362,7 @@ void FreespacePlannerNode::onTimer() is_new_parking_cycle_ = false; // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); From e96b361fc3d6f49bc4b097ca402ffbd3b590bb89 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 11:24:36 +0900 Subject: [PATCH 14/59] feat(autoware_scenario_selector): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_scenario_selector (#9914) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_scenario_selector Signed-off-by: vish0012 --- .../include/autoware/scenario_selector/node.hpp | 5 +++-- planning/autoware_scenario_selector/src/node.cpp | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index 244876ee2cb22..7e215fcfef8dd 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -19,13 +19,13 @@ #include #include +#include #include #include #include #include #include #include -#include #include #include @@ -97,7 +97,8 @@ class ScenarioSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_parking_trajectory_; rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // polling subscribers universe_utils::InterProcessPollingSubscriber< diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index defbde4f6aabc..91e86bfea6dba 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -390,7 +390,7 @@ void ScenarioSelectorNode::onTimer() pub_scenario_->publish(scenario); // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); @@ -490,8 +490,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); published_time_publisher_ = std::make_unique(this); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); } } // namespace autoware::scenario_selector From 9b8b3fc3280a678ea0205009339c587b5febf096 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 15:44:22 +0900 Subject: [PATCH 15/59] =?UTF-8?q?feat(autoware=5Foccupancy=5Fgrid=5Fmap=5F?= =?UTF-8?q?outlier=5Ffilter):=20tier4=5Fdebug=5Fmsgs=20changed=20to=20auto?= =?UTF-8?q?ware=5Finternal=5Fdebug=5Fmsgs=20in=20fil=E2=80=A6=20(#9894)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_occupancy_grid_map_outlier_filter Signed-off-by: vish0012 Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- .../src/occupancy_grid_map_outlier_filter_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp index eb52b903b90ca..094825056e62c 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp +++ b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp @@ -420,9 +420,9 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } From b7ce209a911eac03593240b8b42676b688991626 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 15:47:29 +0900 Subject: [PATCH 16/59] =?UTF-8?q?feat(autoware=5Flidar=5Fapollo=5Finstance?= =?UTF-8?q?=5Fsegmentation):=20tier4=5Fdebug=5Fmsgs=20to=20autoware=5Finte?= =?UTF-8?q?rnal=5Fdebug=5Fmsgs=20in=20files=20perce=E2=80=A6=20(#9876)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat: tier4_debug_msgs to autoware_internal_debug_msgs in files perception/autoware_lidar_apollo_instance_segmentation Signed-off-by: vish0012 Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- .../CMakeLists.txt | 4 ++-- .../autoware_lidar_apollo_instance_segmentation/package.xml | 2 +- .../autoware_lidar_apollo_instance_segmentation/src/node.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt index 16ab7ecccae60..0555cedc235d9 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(tf2_eigen REQUIRED) find_package(autoware_universe_utils REQUIRED) -find_package(tier4_debug_msgs REQUIRED) +find_package(autoware_internal_debug_msgs REQUIRED) find_package(tier4_perception_msgs REQUIRED) find_package(autoware_tensorrt_common) @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} rclcpp_components::component tf2_eigen::tf2_eigen ${autoware_tensorrt_common_TARGETS} - ${tier4_debug_msgs_TARGETS} + ${autoware_internal_debug_msgs_TARGETS} ${tier4_perception_msgs_TARGETS} ) diff --git a/perception/autoware_lidar_apollo_instance_segmentation/package.xml b/perception/autoware_lidar_apollo_instance_segmentation/package.xml index 8a3d5a7ea6f76..8ff7ce77d943d 100755 --- a/perception/autoware_lidar_apollo_instance_segmentation/package.xml +++ b/perception/autoware_lidar_apollo_instance_segmentation/package.xml @@ -14,6 +14,7 @@ ament_cmake autoware_cuda_utils + autoware_internal_debug_msgs autoware_perception_msgs autoware_tensorrt_common autoware_universe_utils @@ -22,7 +23,6 @@ rclcpp rclcpp_components tf2_eigen - tier4_debug_msgs tier4_perception_msgs ament_lint_auto diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp index 9a3e13b81120f..c535fc6762e1f 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp @@ -62,9 +62,9 @@ void LidarInstanceSegmentationNode::pointCloudCallback( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } From acaa40c95a109e21687fe7f2e7f4b37f2ebb1d6b Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 15:50:03 +0900 Subject: [PATCH 17/59] =?UTF-8?q?feat(autoware=5Fground=5Fsegmentation):?= =?UTF-8?q?=20tier4=5Fdebug=5Fmsgs=20changed=20to=20autoware=5Finternal=5F?= =?UTF-8?q?debug=5Fmsgs=20in=20fil=E2=80=A6=20(#9878)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_ground_segmentation Signed-off-by: vish0012 --- .../src/scan_ground_filter/node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp index 74b4edbe595ea..8fda0c1395caf 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -369,9 +369,9 @@ void ScanGroundFilterComponent::faster_filter( if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); } } From b65e042ebd1df00a09d6a47648dfb07df3e0f68a Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 15:50:22 +0900 Subject: [PATCH 18/59] feat(autoware_image_diagnostics): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_image_diagnostics (#9918) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files sensing/autoware_image_diagnostics Signed-off-by: vish0012 --- sensing/autoware_image_diagnostics/README.md | 14 +++++++------- sensing/autoware_image_diagnostics/package.xml | 2 +- .../src/image_diagnostics_node.cpp | 4 ++-- .../src/image_diagnostics_node.hpp | 11 ++++++----- 4 files changed, 16 insertions(+), 15 deletions(-) diff --git a/sensing/autoware_image_diagnostics/README.md b/sensing/autoware_image_diagnostics/README.md index 03858088564b3..42fcbdc8dc612 100644 --- a/sensing/autoware_image_diagnostics/README.md +++ b/sensing/autoware_image_diagnostics/README.md @@ -28,13 +28,13 @@ After all image's blocks state are evaluated, the whole image status is summariz ### Output -| Name | Type | Description | -| ----------------------------------- | --------------------------------------- | ------------------------------------- | -| `image_diag/debug/gray_image` | `sensor_msgs::msg::Image` | gray image | -| `image_diag/debug/dft_image` | `sensor_msgs::msg::Image` | discrete Fourier transformation image | -| `image_diag/debug/diag_block_image` | `sensor_msgs::msg::Image` | each block state colorization | -| `image_diag/image_state_diag` | `tier4_debug_msgs::msg::Int32Stamped` | image diagnostics status value | -| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | +| Name | Type | Description | +| ----------------------------------- | ------------------------------------------------- | ------------------------------------- | +| `image_diag/debug/gray_image` | `sensor_msgs::msg::Image` | gray image | +| `image_diag/debug/dft_image` | `sensor_msgs::msg::Image` | discrete Fourier transformation image | +| `image_diag/debug/diag_block_image` | `sensor_msgs::msg::Image` | each block state colorization | +| `image_diag/image_state_diag` | `autoware_internal_debug_msgs::msg::Int32Stamped` | image diagnostics status value | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | ## Parameters diff --git a/sensing/autoware_image_diagnostics/package.xml b/sensing/autoware_image_diagnostics/package.xml index 2e4556de2b92b..3140b87e1f1b9 100644 --- a/sensing/autoware_image_diagnostics/package.xml +++ b/sensing/autoware_image_diagnostics/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto + autoware_internal_debug_msgs autoware_universe_utils cv_bridge diagnostic_updater @@ -19,7 +20,6 @@ rclcpp rclcpp_components sensor_msgs - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp index 1625cba5b72aa..57e53aee46328 100644 --- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp @@ -33,7 +33,7 @@ ImageDiagNode::ImageDiagNode(const rclcpp::NodeOptions & node_options) dft_image_pub_ = image_transport::create_publisher(this, "image_diag/debug/dft_image"); gray_image_pub_ = image_transport::create_publisher(this, "image_diag/debug/gray_image"); - image_state_pub_ = create_publisher( + image_state_pub_ = create_publisher( "image_diag/image_state_diag", rclcpp::SensorDataQoS()); updater_.setHardwareID("Image_Diagnostics"); @@ -225,7 +225,7 @@ void ImageDiagNode::ImageChecker(const sensor_msgs::msg::Image::ConstSharedPtr i } else { params_.diagnostic_status = 0; } - tier4_debug_msgs::msg::Int32Stamped image_state_out; + autoware_internal_debug_msgs::msg::Int32Stamped image_state_out; image_state_out.data = params_.diagnostic_status; image_state_pub_->publish(image_state_out); diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp index 325624062b90b..9383da33160f9 100644 --- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp @@ -21,10 +21,10 @@ #include #include +#include +#include +#include #include -#include -#include -#include #if __has_include() #include @@ -95,8 +95,9 @@ class ImageDiagNode : public rclcpp::Node image_transport::Publisher block_diag_image_pub_; image_transport::Publisher dft_image_pub_; image_transport::Publisher gray_image_pub_; - rclcpp::Publisher::SharedPtr average_pub_; - rclcpp::Publisher::SharedPtr image_state_pub_; + rclcpp::Publisher::SharedPtr + average_pub_; + rclcpp::Publisher::SharedPtr image_state_pub_; }; } // namespace autoware::image_diagnostics From 221a7bc0eb333f27638d5bdc5a9d7ecc4fd79be0 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 15:50:56 +0900 Subject: [PATCH 19/59] =?UTF-8?q?feat(autoware=5Fcompare=5Fmap=5Fsegmentat?= =?UTF-8?q?ion):=20tier4=5Fdebug=5Fmsgs=20changed=20to=20autoware=5Fintern?= =?UTF-8?q?al=5Fdebug=5Fmsgs=20in=20fil=E2=80=A6=20(#9869)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_compare_map_segmentation Signed-off-by: vish0012 Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- .../src/distance_based_compare_map_filter/node.cpp | 4 ++-- .../src/voxel_based_approximate_compare_map_filter/node.cpp | 4 ++-- .../src/voxel_based_compare_map_filter/node.cpp | 4 ++-- .../src/voxel_distance_based_compare_map_filter/node.cpp | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp index ae07d54ad53d6..ac759504de557 100644 --- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp @@ -173,9 +173,9 @@ void DistanceBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index 9f325b36676be..5794cfe37a45b 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -141,9 +141,9 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index a31d5ec5dd6d6..db5e46a0eff5a 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp @@ -100,9 +100,9 @@ void VoxelBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp index d0bcf381cd62f..775a931ac4c4c 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -173,9 +173,9 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } From cee6ef912154312c3a4e6eff3ee04013cb9d7175 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 15:54:14 +0900 Subject: [PATCH 20/59] feat(autoware_probabilistic_occupancy_grid_map): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_probabilistic_occupancy_grid_map (#9895) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_probabilistic_occupancy_grid_map Signed-off-by: vish0012 Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- .../src/fusion/synchronized_grid_map_fusion_node.cpp | 6 +++--- .../pointcloud_based_occupancy_grid_map_node.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 880dfda03c495..7cde04ca9614b 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -332,11 +332,11 @@ void GridMapFusionNode::publish() std::chrono::duration( std::chrono::nanoseconds((this->get_clock()->now() - latest_stamp).nanoseconds())) .count(); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 4d442ad2c33db..ff836a1ddf760 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -261,11 +261,11 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( std::chrono::nanoseconds( (this->get_clock()->now() - input_raw_msg->header.stamp).nanoseconds())) .count(); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } From 95e7939e7e3b6164913ce5b5119247de94a89ac8 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Wed, 15 Jan 2025 16:06:17 +0900 Subject: [PATCH 21/59] feat(autoware_pointcloud_preprocessor): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_pointcloud_preprocessor (#9920) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files sensing/autoware_pointcloud_preprocessor Signed-off-by: vish0012 --- .../docs/blockage-diag.md | 20 +++++++++---------- .../docs/dual-return-outlier-filter.md | 10 +++++----- .../blockage_diag/blockage_diag_node.hpp | 13 +++++++----- .../concatenate_and_time_sync_nodelet.hpp | 4 ++-- .../concatenate_pointclouds.hpp | 4 ++-- .../dual_return_outlier_filter_node.hpp | 4 ++-- .../ring_outlier_filter_node.hpp | 2 +- .../time_synchronizer_node.hpp | 4 ++-- .../package.xml | 2 +- .../src/blockage_diag/blockage_diag_node.cpp | 12 +++++------ .../concatenate_and_time_sync_nodelet.cpp | 6 +++--- .../concatenate_pointclouds.cpp | 6 +++--- .../crop_box_filter/crop_box_filter_node.cpp | 6 +++--- .../distortion_corrector_node.cpp | 6 +++--- ...ased_voxel_grid_downsample_filter_node.cpp | 6 +++--- .../dual_return_outlier_filter_node.cpp | 4 ++-- .../ring_outlier_filter_node.cpp | 10 +++++----- .../time_synchronizer_node.cpp | 6 +++--- 18 files changed, 64 insertions(+), 61 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md b/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md index 911cda3823021..a9dcb8437ec67 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md @@ -38,16 +38,16 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Output -| Name | Type | Description | -| :-------------------------------------------------------- | :-------------------------------------- | ------------------------------------------------------------------------------------------------ | -| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | -| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | -| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | -| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | -| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | -| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | -| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | -| `~/output/blockage_diag/debug/ground_dust_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | +| Name | Type | Description | +| :-------------------------------------------------------- | :-------------------------------------------------- | ------------------------------------------------------------------------------------------------ | +| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | +| `~/output/blockage_diag/debug/ground_blockage_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | +| `~/output/blockage_diag/debug/sky_blockage_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | +| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | +| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | +| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | +| `~/output/blockage_diag/debug/ground_dust_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | ## Parameters diff --git a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md index 8f4da273861a3..0d0423d40ab83 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -36,11 +36,11 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Output -| Name | Type | Description | -| ---------------------------------------------- | --------------------------------------- | ------------------------------------------------------- | -| `/dual_return_outlier_filter/frequency_image` | `sensor_msgs::msg::Image` | The histogram image that represent visibility | -| `/dual_return_outlier_filter/visibility` | `tier4_debug_msgs::msg::Float32Stamped` | A representation of visibility with a value from 0 to 1 | -| `/dual_return_outlier_filter/pointcloud_noise` | `sensor_msgs::msg::Pointcloud2` | The pointcloud removed as noise | +| Name | Type | Description | +| ---------------------------------------------- | --------------------------------------------------- | ------------------------------------------------------- | +| `/dual_return_outlier_filter/frequency_image` | `sensor_msgs::msg::Image` | The histogram image that represent visibility | +| `/dual_return_outlier_filter/visibility` | `autoware_internal_debug_msgs::msg::Float32Stamped` | A representation of visibility with a value from 0 to 1 | +| `/dual_return_outlier_filter/pointcloud_noise` | `sensor_msgs::msg::Pointcloud2` | The pointcloud removed as noise | ## Parameters diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp index 3601b492c4fe3..2dbda770f6cec 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp @@ -22,10 +22,10 @@ #include #include +#include #include #include #include -#include #if __has_include() #include @@ -58,10 +58,13 @@ class BlockageDiagComponent : public autoware::pointcloud_preprocessor::Filter image_transport::Publisher single_frame_dust_mask_pub; image_transport::Publisher multi_frame_dust_mask_pub; image_transport::Publisher blockage_dust_merged_pub; - rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; - rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; - rclcpp::Publisher::SharedPtr ground_dust_ratio_pub_; - rclcpp::Publisher::SharedPtr blockage_type_pub_; + rclcpp::Publisher::SharedPtr + ground_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr + sky_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr + ground_dust_ratio_pub_; + rclcpp::Publisher::SharedPtr blockage_type_pub_; private: void onBlockageChecker(DiagnosticStatusWrapper & stat); diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 40adad3521f8d..6164e85c35717 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -69,14 +69,14 @@ #include #include +#include +#include #include #include #include #include #include #include -#include -#include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index 0e959eedae1b6..d57e44431a8d2 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -68,13 +68,13 @@ #include #include +#include +#include #include #include #include #include #include -#include -#include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp index ef33e88ef5c98..fa55222fb6530 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp @@ -22,9 +22,9 @@ #include #include +#include #include #include -#include #if __has_include() #include @@ -54,7 +54,7 @@ class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocesso /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher image_pub_; - rclcpp::Publisher::SharedPtr visibility_pub_; + rclcpp::Publisher::SharedPtr visibility_pub_; rclcpp::Publisher::SharedPtr noise_cloud_pub_; private: diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp index c898cbacf33ea..96bd2fb2411a0 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp @@ -53,7 +53,7 @@ class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Fil const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, const TransformInfo & transform_info) override; - rclcpp::Publisher::SharedPtr visibility_pub_; + rclcpp::Publisher::SharedPtr visibility_pub_; private: /** \brief publisher of excluded pointcloud for debug reason. **/ diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp index d36fcb36a7be1..f4921be66a7e2 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp @@ -68,14 +68,14 @@ #include #include +#include +#include #include #include #include #include #include #include -#include -#include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/package.xml b/sensing/autoware_pointcloud_preprocessor/package.xml index 4511c5497a3a0..525b1f3eb0699 100644 --- a/sensing/autoware_pointcloud_preprocessor/package.xml +++ b/sensing/autoware_pointcloud_preprocessor/package.xml @@ -26,6 +26,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_pcl_extensions autoware_point_types @@ -51,7 +52,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp index 5021a3551c939..e05d5f789f680 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp @@ -68,7 +68,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options std::string(this->get_namespace()) + ": dust_validation", this, &BlockageDiagComponent::dustChecker); - ground_dust_ratio_pub_ = create_publisher( + ground_dust_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); if (publish_debug_image_) { single_frame_dust_mask_pub = @@ -86,9 +86,9 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); } - ground_blockage_ratio_pub_ = create_publisher( + ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); - sky_blockage_ratio_pub_ = create_publisher( + sky_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/sky_blockage_ratio", rclcpp::SensorDataQoS()); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( @@ -315,7 +315,7 @@ void BlockageDiagComponent::filter( cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1); cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img); - tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; ground_dust_ratio_ = static_cast(cv::countNonZero(single_dust_ground_img)) / (single_dust_ground_img.cols * single_dust_ground_img.rows); ground_dust_ratio_msg.data = ground_dust_ratio_; @@ -386,12 +386,12 @@ void BlockageDiagComponent::filter( } } - tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; ground_blockage_ratio_msg.data = ground_blockage_ratio_; ground_blockage_ratio_msg.stamp = now(); ground_blockage_ratio_pub_->publish(ground_blockage_ratio_msg); - tier4_debug_msgs::msg::Float32Stamped sky_blockage_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped sky_blockage_ratio_msg; sky_blockage_ratio_msg.data = sky_blockage_ratio_; sky_blockage_ratio_msg.stamp = now(); sky_blockage_ratio_pub_->publish(sky_blockage_ratio_msg); diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index f8baae3405873..b49e21b8f30be 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -455,9 +455,9 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } for (const auto & e : cloud_stdmap_) { @@ -468,7 +468,7 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() std::chrono::nanoseconds( (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index ba29389b88bf7..ba5e2eb51997a 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -271,7 +271,7 @@ void PointCloudConcatenationComponent::publish() std::chrono::nanoseconds( (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); } } @@ -297,9 +297,9 @@ void PointCloudConcatenationComponent::publish() if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp index ecbc5b8fd13ef..54c940d414db8 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp @@ -194,9 +194,9 @@ void CropBoxFilterComponent::faster_filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -204,7 +204,7 @@ void CropBoxFilterComponent::faster_filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 9eaafeae9dbc7..cc05a6cc2765c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -123,7 +123,7 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po std::chrono::nanoseconds( (this->get_clock()->now() - pointcloud_msg->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } @@ -133,9 +133,9 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp index 3d2bc0a206c94..0c467234d6591 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp @@ -142,9 +142,9 @@ void PickupBasedVoxelGridDownsampleFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -152,7 +152,7 @@ void PickupBasedVoxelGridDownsampleFilterComponent::filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp index a8024e02de2c1..bac3a59e09528 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp @@ -65,7 +65,7 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( image_pub_ = image_transport::create_publisher(this, "dual_return_outlier_filter/debug/frequency_image"); - visibility_pub_ = create_publisher( + visibility_pub_ = create_publisher( "dual_return_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); { rclcpp::PublisherOptions pub_options; @@ -322,7 +322,7 @@ void DualReturnOutlierFilterComponent::filter( frequency_image_msg->header = input->header; // Publish histogram image image_pub_.publish(frequency_image_msg); - tier4_debug_msgs::msg::Float32Stamped visibility_msg; + autoware_internal_debug_msgs::msg::Float32Stamped visibility_msg; visibility_msg.data = (1.0f - filled); visibility_msg.stamp = now(); visibility_pub_->publish(visibility_msg); diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index fae9043143ba4..fcea299ba5aff 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -41,7 +41,7 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions outlier_pointcloud_publisher_ = this->create_publisher("debug/ring_outlier_filter", 1, pub_options); } - visibility_pub_ = create_publisher( + visibility_pub_ = create_publisher( "ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); @@ -262,7 +262,7 @@ void RingOutlierFilterComponent::faster_filter( outlier.header = input->header; outlier_pointcloud_publisher_->publish(outlier); - tier4_debug_msgs::msg::Float32Stamped visibility_msg; + autoware_internal_debug_msgs::msg::Float32Stamped visibility_msg; visibility_msg.data = calculateVisibilityScore(outlier); visibility_msg.stamp = input->header.stamp; visibility_pub_->publish(visibility_msg); @@ -272,9 +272,9 @@ void RingOutlierFilterComponent::faster_filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -282,7 +282,7 @@ void RingOutlierFilterComponent::faster_filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp index 42170fd88ee36..e5bd4a955590f 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp @@ -378,7 +378,7 @@ void PointCloudDataSynchronizerComponent::publish() std::chrono::nanoseconds( (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); } auto output = std::make_unique(*e.second); @@ -400,9 +400,9 @@ void PointCloudDataSynchronizerComponent::publish() if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } From b20d7f05df2da1dd1580e824c82316ed77f49059 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 15 Jan 2025 16:20:10 +0900 Subject: [PATCH 22/59] chore(autoware_test_utils): move rviz config to autoware_launch (#9925) Signed-off-by: Mamoru Sobue --- common/autoware_test_utils/CMakeLists.txt | 1 - .../launch/psim_intersection.launch.xml | 2 +- .../launch/psim_road_shoulder.launch.xml | 2 +- .../rviz/psim_test_map.rviz | 4569 ----------------- 4 files changed, 2 insertions(+), 4572 deletions(-) delete mode 100644 common/autoware_test_utils/rviz/psim_test_map.rviz diff --git a/common/autoware_test_utils/CMakeLists.txt b/common/autoware_test_utils/CMakeLists.txt index 8bfa44f7bf0ba..20b8f964f879f 100644 --- a/common/autoware_test_utils/CMakeLists.txt +++ b/common/autoware_test_utils/CMakeLists.txt @@ -25,5 +25,4 @@ ament_auto_package(INSTALL_TO_SHARE test_map test_data launch - rviz ) diff --git a/common/autoware_test_utils/launch/psim_intersection.launch.xml b/common/autoware_test_utils/launch/psim_intersection.launch.xml index 723dd7352219d..dc3e75cca1d96 100644 --- a/common/autoware_test_utils/launch/psim_intersection.launch.xml +++ b/common/autoware_test_utils/launch/psim_intersection.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml index 7dd888a036f9d..58b71524e72b8 100644 --- a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml +++ b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/common/autoware_test_utils/rviz/psim_test_map.rviz b/common/autoware_test_utils/rviz/psim_test_map.rviz deleted file mode 100644 index 8475c94b86bb4..0000000000000 --- a/common/autoware_test_utils/rviz/psim_test_map.rviz +++ /dev/null @@ -1,4569 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - Splitter Ratio: 0.557669460773468 - Tree Height: 225 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: AutowareDateTimePanel - Name: AutowareDateTimePanel - - Class: rviz_plugins::AutowareStatePanel - Name: AutowareStatePanel - - Class: rviz_plugins::SimulatedClockPanel - Name: SimulatedClockPanel - - Class: rviz_plugins::TrafficLightPublishPanel - Name: TrafficLightPublishPanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Class: rviz_common/Group - Displays: - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - ars408_front_center: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera0/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera0/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera1/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera1/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera2/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera2/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera3/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera3/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera4/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera4/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera5/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera5/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera6/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera6/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera7/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera7/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - gnss_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - livox_front_left: - Alpha: 1 - Show Axes: false - Show Trail: false - livox_front_left_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - livox_front_right: - Alpha: 1 - Show Axes: false - Show Trail: false - livox_front_right_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_kit_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tamagawa/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_left: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_left_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_rear: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_rear_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_right: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_right_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_top: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_top_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Mass Properties: - Inertia: false - Mass: false - Name: VehicleModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Background Alpha: 0.5 - Background Color: 23; 28; 31 - Class: autoware_overlay_rviz_plugin/SignalDisplay - Dark Traffic Color: 255; 51; 51 - Enabled: true - Gear Topic Test: /vehicle/status/gear_status - Handle Angle Scale: 17 - Hazard Lights Topic: /vehicle/status/hazard_lights_status - Height: 100 - Left: 0 - Light Traffic Color: 255; 153; 153 - Name: SignalDisplay - Primary Color: 174; 174; 174 - Signal Color: 0; 230; 120 - Speed Limit Topic: /planning/scenario_planning/current_max_velocity - Speed Topic: /vehicle/status/velocity_status - Steering Topic: /vehicle/status/steering_status - Top: 10 - Traffic Topic: /planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal - Turn Signals Topic: /vehicle/status/turn_indicators_status - Value: true - Width: 550 - Enabled: true - Name: Vehicle - - Class: rviz_plugins/MrmSummaryOverlayDisplay - Enabled: false - Font Size: 10 - Left: 512 - Max Letter Num: 100 - Name: MRM Summary - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /system/emergency/hazard_status - Value: false - Value height offset: 0 - Enabled: true - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - bus_stop_area: true - center_lane_line: false - center_line_arrows: false - curbstone: true - lane_start_bound: false - lanelet direction: true - lanelet_id: false - left_lane_bound: true - no_parking_area: true - parking_lots: true - parking_space: true - partitions: true - right_lane_bound: true - road_lanelets: false - shoulder_center_lane_line: false - shoulder_center_line_arrows: true - shoulder_lane_start_bound: true - shoulder_left_lane_bound: true - shoulder_right_lane_bound: true - shoulder_road_lanelets: false - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: true - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - Alpha: 0.9990000128746033 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: MeasurementRange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/lidar/crop_box_filter/crop_box_polygon - Value: false - Enabled: true - Name: LiDAR - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 233; 185; 110 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: false - Position: - Alpha: 0.20000000298023224 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.699999988079071 - Head Radius: 1.2000000476837158 - Name: PoseWithCovariance - Shaft Length: 1 - Shaft Radius: 0.5 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/gnss/pose_with_covariance - Value: true - Enabled: false - Name: GNSS - Enabled: true - Name: Sensing - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 170; 255 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovInitial - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/initial_pose_with_covariance - Value: false - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 255; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovAligned - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose_with_covariance - Value: false - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: false - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /localization/util/downsample/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Aligned - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MonteCarloInitialPose - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/monte_carlo_initial_pose_marker - Value: true - Enabled: true - Name: NDT - - Class: rviz_common/Group - Displays: - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: EKF - Enabled: true - Name: Localization - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 15 - Min Value: -2 - Value: false - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 200; 200; 200 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15 - Min Color: 0; 0; 0 - Min Intensity: -5 - Name: NoGroundPointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/obstacle_segmentation/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Segmentation - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Detection - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/TrackedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Dynamic Status: All - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: TrackedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Tracking - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/PredictedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: PredictedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Maneuver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/prediction/maneuver - Value: false - Enabled: true - Name: Prediction - Enabled: true - Name: ObjectRecognition - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RecognitionResultOnImage - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light/debug/rois - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MapBasedDetectionResult - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers - Value: true - Enabled: true - Name: TrafficLight - - Class: rviz_common/Group - Displays: - - Alpha: 0.5 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map_updates - Use Timestamp: false - Value: true - Enabled: false - Name: OccupancyGrid - Enabled: true - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: RouteArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/route_marker - Value: true - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.5 - Name: GoalPose - Shaft Length: 3 - Shaft Radius: 0.20000000298023224 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/echo_back_goal_pose - Value: true - - Background Alpha: 0.5 - Background Color: 23; 28; 31 - Class: autoware_mission_details_overlay_rviz_plugin/MissionDetailsDisplay - Enabled: true - Height: 100 - Name: MissionDetailsDisplay - Remaining Distance and Time Topic: /planning/mission_remaining_distance_time - Right: 10 - Text Color: 194; 194; 194 - Top: 10 - Value: true - Width: 170 - Enabled: true - Name: MissionPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: ScenarioTrajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/trajectory - Value: true - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.9990000128746033 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: Path - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/path - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.4000000059604645 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_AvoidanceByLC - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/avoidance_by_lane_change - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_Avoidance - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/static_obstacle_avoidance - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_LaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/lane_change_right - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_LaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/lane_change_left - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_GoalPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/goal_planner - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_StartPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/start_planner - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_AvoidanceByLC - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/avoidance_by_lane_change - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: (old)PathChangeCandidate_LaneChange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_LaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change_right - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_LaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change_left - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_ExternalRequestLaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/external_request_lane_change_right - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_ExternalRequestLaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/external_request_lane_change_left - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_Avoidance - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/static_obstacle_avoidance - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_StartPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/start_planner - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_GoalPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/goal_planner - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Bound - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StaticObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/static_obstacle_avoidance - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (AvoidanceByLC) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/avoidance_by_lane_change - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (LaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (LaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ExtLaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ExtLaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (GoalPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/goal_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StartPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/start_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (BlindSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Crosswalk) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Walkway) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/walkway - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DetectionArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Intersection) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MergeFromPrivateArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoStoppingArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OcclusionSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StopLine) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (TrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (VirtualTrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (RunOut) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SpeedBump) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/speed_bump - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoDrivableLane) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_drivable_lane - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Arrow - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Blind Spot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: VirtualTrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StopLine - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: OcclusionSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RunOut - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StaticObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/static_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LeftLaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RightLaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneFollowing - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_following - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: GoalPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/goal_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SideShift - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/side_shift - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SpeedBump - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/speed_bump - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DynamicObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/dynamic_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoDrivableLane - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_drivable_lane - Value: false - Enabled: false - Name: DebugMarker - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (StaticObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/static_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (AvoidanceByLC) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/avoidance_by_lane_change - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (LaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (LaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (ExtLaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (ExtLaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (GoalPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/goal_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (StartPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (DynamicObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/dynamic_obstacle_avoidance - Value: false - Enabled: false - Name: InfoMarker - Enabled: true - Name: BehaviorPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/trajectory - Value: false - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.9990000128746033 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SurroundObstacle) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleCruise) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MotionVelocitySmoother) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/velocity_smoother/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OutOfLane) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/virtual_walls - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleVelocityLimiter) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_velocity_limiter/virtual_walls - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DynamicObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/dynamic_obstacle_stop/virtual_walls - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SurroundObstacleCheck - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Footprint - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 239; 41; 41 - Enabled: false - Name: FootprintOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 10; 21; 255 - Enabled: false - Name: FootprintRecoverOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset - Value: false - Enabled: false - Name: SurroundObstacleChecker - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: CruiseVirtualWall - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SlowDownVirtualWall - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/slow_down/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DebugMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker - Value: true - Enabled: false - Name: ObstacleCruise - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: OutOfLane - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/debug_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleVelocityLimiter - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_velocity_limiter/debug_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DynamicObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/dynamic_obstacle_stop/debug_markers - Value: true - Enabled: false - Name: MotionVelocityPlanner - Enabled: false - Name: DebugMarker - Enabled: true - Name: MotionPlanning - Enabled: true - Name: LaneDriving - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: false - Name: Costmap - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates - Use Timestamp: false - Value: false - - Alpha: 0.9990000128746033 - Arrow Length: 0.30000001192092896 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PartialPoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (3D) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array - Value: true - - Alpha: 0.9990000128746033 - Arrow Length: 0.5 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 0; 0; 255 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array - Value: true - Enabled: true - Name: Parking - - Class: rviz_plugins/PoseWithUuidStamped - Enabled: true - Length: 1.5 - Name: ModifiedGoal - Radius: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/modified_goal - UUID: - Scale: 0.30000001192092896 - Value: false - Value: true - Enabled: true - Name: ScenarioPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: PlanningErrorMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker - Value: true - Enabled: false - Name: Diagnostic - Enabled: true - Name: Planning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Predicted Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/predicted_trajectory_in_frenet_coordinate - Value: true - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Constant Width: true - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 0.05000000074505806 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Resampled Reference Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/resampled_reference_trajectory - Value: false - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Constant Width: true - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: false - Width: 0.20000000298023224 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: true - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (AEB) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/autonomous_emergency_braking/virtual_wall - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Stop Reason - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/longitudinal/stop_reason - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/MPC - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/mpc_follower/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/PurePursuit - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/AEB - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/autonomous_emergency_braking/debug/markers - Value: false - Enabled: true - Name: Control - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: ~ - Enabled: true - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Camera - Enabled: true - Far Plane Distance: 100 - Image Rendering: background and overlay - Name: PointcloudOnCamera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera6/image_raw - Value: true - Visibility: - Control: - Debug/AEB: true - Debug/MPC: true - Debug/PurePursuit: true - Predicted Trajectory: true - Resampled Reference Trajectory: true - Stop Reason: true - Value: false - VirtualWall: - Value: true - VirtualWall (AEB): true - Debug: - Control: true - Localization: - "": true - Value: true - Map: true - Perception: - "": true - Value: true - Planning: - "": - "": true - Value: true - Value: true - Sensing: - ConcatenatePointCloud: true - RadarRawObjects(white): true - Value: true - Value: true - Localization: - EKF: - PoseHistory: true - Value: true - NDT: - Aligned: true - Initial: true - MonteCarloInitialPose: true - PoseHistory: true - PoseWithCovAligned: true - PoseWithCovInitial: true - Value: true - Value: false - Map: - Lanelet2VectorMap: true - PointCloudMap: true - Value: false - Perception: - ObjectRecognition: - Detection: - DetectedObjects: true - Value: true - Prediction: - Maneuver: true - PredictedObjects: true - Value: true - Tracking: - TrackedObjects: true - Value: true - Value: true - OccupancyGrid: - Map: true - Value: true - Segmentation: - NoGroundPointCloud: true - Value: true - TrafficLight: - MapBasedDetectionResult: true - RecognitionResultOnImage: true - Value: true - Value: false - Planning: - Diagnostic: - PlanningErrorMarker: true - Value: true - MissionPlanning: - GoalPose: true - MissionDetailsDisplay: true - RouteArea: true - Value: true - ScenarioPlanning: - LaneDriving: - BehaviorPlanning: - (old)PathChangeCandidate_LaneChange: true - Bound: true - DebugMarker: - Arrow: true - Blind Spot: true - Crosswalk: true - DetectionArea: true - DynamicObstacleAvoidance: true - GoalPlanner: true - Intersection: true - LaneFollowing: true - LeftLaneChange: true - NoDrivableLane: true - NoStoppingArea: true - OcclusionSpot: true - RightLaneChange: true - RunOut: true - SideShift: true - SpeedBump: true - StartPlanner: true - StaticObstacleAvoidance: true - StopLine: true - TrafficLight: true - Value: true - VirtualTrafficLight: true - InfoMarker: - Info (AvoidanceByLC): true - Info (DynamicObstacleAvoidance): true - Info (ExtLaneChangeLeft): true - Info (ExtLaneChangeRight): true - Info (GoalPlanner): true - Info (LaneChangeLeft): true - Info (LaneChangeRight): true - Info (StartPlanner): true - Info (StaticObstacleAvoidance): true - Value: true - Path: true - PathChangeCandidate_Avoidance: true - PathChangeCandidate_AvoidanceByLC: true - PathChangeCandidate_ExternalRequestLaneChangeLeft: true - PathChangeCandidate_ExternalRequestLaneChangeRight: true - PathChangeCandidate_GoalPlanner: true - PathChangeCandidate_LaneChangeLeft: true - PathChangeCandidate_LaneChangeRight: true - PathChangeCandidate_StartPlanner: true - PathReference_Avoidance: true - PathReference_AvoidanceByLC: true - PathReference_GoalPlanner: true - PathReference_LaneChangeLeft: true - PathReference_LaneChangeRight: true - PathReference_StartPlanner: true - Value: true - VirtualWall: - Value: true - VirtualWall (AvoidanceByLC): true - VirtualWall (BlindSpot): true - VirtualWall (Crosswalk): true - VirtualWall (DetectionArea): true - VirtualWall (ExtLaneChangeLeft): true - VirtualWall (ExtLaneChangeRight): true - VirtualWall (GoalPlanner): true - VirtualWall (Intersection): true - VirtualWall (LaneChangeLeft): true - VirtualWall (LaneChangeRight): true - VirtualWall (MergeFromPrivateArea): true - VirtualWall (NoDrivableLane): true - VirtualWall (NoStoppingArea): true - VirtualWall (OcclusionSpot): true - VirtualWall (RunOut): true - VirtualWall (SpeedBump): true - VirtualWall (StartPlanner): true - VirtualWall (StaticObstacleAvoidance): true - VirtualWall (StopLine): true - VirtualWall (TrafficLight): true - VirtualWall (VirtualTrafficLight): true - VirtualWall (Walkway): true - MotionPlanning: - DebugMarker: - MotionVelocityPlanner: - DynamicObstacleStop: true - ObstacleVelocityLimiter: true - OutOfLane: true - Value: true - ObstacleAvoidance: true - ObstacleCruise: - CruiseVirtualWall: true - DebugMarker: true - SlowDownVirtualWall: true - Value: true - ObstacleStop: true - SurroundObstacleChecker: - Footprint: true - FootprintOffset: true - FootprintRecoverOffset: true - SurroundObstacleCheck: true - Value: true - Value: true - Trajectory: true - Value: true - VirtualWall: - Value: true - VirtualWall (DynamicObstacleStop): true - VirtualWall (MotionVelocitySmoother): true - VirtualWall (ObstacleAvoidance): true - VirtualWall (ObstacleCruise): true - VirtualWall (ObstacleStop): true - VirtualWall (ObstacleVelocityLimiter): true - VirtualWall (OutOfLane): true - VirtualWall (SurroundObstacle): true - Value: true - ModifiedGoal: true - Parking: - Costmap: true - PartialPoseArray: true - PoseArray: true - Value: true - ScenarioTrajectory: true - Value: true - Value: false - Sensing: - GNSS: - PoseWithCovariance: true - Value: true - LiDAR: - ConcatenatePointCloud: true - MeasurementRange: true - Value: true - Value: true - System: - Grid: true - MRM Summary: true - TF: true - Value: false - Vehicle: - PolarGridDisplay: true - SignalDisplay: true - Value: true - VehicleModel: true - Value: true - Zoom Factor: 1 - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - BUS: - Alpha: 0.5 - Color: 255; 255; 255 - CAR: - Alpha: 0.5 - Color: 255; 255; 255 - CYCLIST: - Alpha: 0.5 - Color: 255; 255; 255 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.5 - Color: 255; 255; 255 - Name: RadarRawObjects(white) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.5 - Color: 255; 255; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.5 - Color: 255; 255; 255 - TRUCK: - Alpha: 0.5 - Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/detected_objects - UNKNOWN: - Alpha: 0.5 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Sensing - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: NDT pointclouds - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: NDTLoadedPCDMap - Position Transformer: "" - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/debug/loaded_pointcloud_map - Use Fixed Frame: true - Use rainbow: true - Value: true - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: NDTPoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: true - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: EKFPoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: Localization - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Name: Centerpoint(red1) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Name: CenterpointROIFusion(red2) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/roi_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Name: CenterpointValidator(red3) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/validation/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - CAR: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Name: PointPainting(light_green1) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - TRUCK: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - CAR: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Name: PointPaintingROIFusion(light_green2) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - TRUCK: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/roi_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - CAR: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Name: PointPaintingValidator(light_green3) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - TRUCK: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/validation/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Name: DetectionByTracker(orange) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/detection_by_tracker/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - CAR: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Name: CameraLidarFusion(purple) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - TRUCK: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/clustering/camera_lidar_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Name: RadarFarObjects(white) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/radar/far_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Name: Detection(yellow) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - CAR: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Class: autoware_perception_rviz_plugin/TrackedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Dynamic Status: All - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Name: Tracking(green) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - TRUCK: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Class: autoware_perception_rviz_plugin/PredictedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Name: Prediction(light_blue) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Value: true - Visualization Type: Normal - Enabled: true - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: LaneChangeLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: LaneChangeRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/static_obstacle_avoidance_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/static_obstacle_avoidance_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceByLCLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/avoidance_by_lc_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceByLCRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/avoidance_by_lc_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/start_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: GoalPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/goal_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: BlindSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/no_stopping_area - Value: true - Enabled: true - Name: Objects Of Interest - Enabled: true - Name: Planning - - Class: rviz_common/Group - Displays: ~ - Enabled: true - Name: Control - Enabled: false - Name: Debug - Enabled: true - Global Options: - Background Color: 15; 20; 23 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Class: tier4_adapi_rviz_plugins::RouteTool - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rviz/routing/rough_goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 3 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareDateTimePanel: - collapsed: false - AutowareStatePanel: - collapsed: false - Displays: - collapsed: false - Height: 939 - Hide Left Dock: false - Hide Right Dock: false - PointcloudOnCamera: - collapsed: false - QMainWindow State: 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 - RecognitionResultOnImage: - collapsed: false - Selection: - collapsed: false - SimulatedClockPanel: - collapsed: false - Tool Properties: - collapsed: false - TrafficLightPublishPanel: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 34 From 87537bd02e6479e0bb5f95d874822c8e81181c13 Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Wed, 15 Jan 2025 18:04:13 +0900 Subject: [PATCH 23/59] chore(autoware_traffic_light_occlusion_predictor): modify docs (#9820) * fix docs Signed-off-by: MasatoSaeki * fix docs Signed-off-by: MasatoSaeki --------- Signed-off-by: MasatoSaeki --- .../README.md | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/perception/autoware_traffic_light_occlusion_predictor/README.md b/perception/autoware_traffic_light_occlusion_predictor/README.md index bc57dbea76c97..5d2e320dfc72a 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/README.md +++ b/perception/autoware_traffic_light_occlusion_predictor/README.md @@ -1,8 +1,10 @@ -# The `autoware_traffic_light_occlusion_predictor` Package +# autoware_traffic_light_occlusion_predictor ## Overview `autoware_traffic_light_occlusion_predictor` receives the detected traffic lights rois and calculates the occlusion ratios of each roi with point cloud. +If that rois is judged as occlusion, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `UNKNOWN`, and `0.0`. +This node publishes when each car and pedestrian `traffic_signals` is arrived and processed. For each traffic light roi, hundreds of pixels would be selected and projected into the 3D space. Then from the camera point of view, the number of projected pixels that are occluded by the point cloud is counted and used for calculating the occlusion ratio for the roi. As shown in follow image, the red pixels are occluded and the occlusion ratio is the number of red pixels divided by the total pixel numbers. @@ -12,18 +14,20 @@ If no point cloud is received or all point clouds have very large stamp differen ## Input topics -| Name | Type | Description | -| -------------------- | ---------------------------------------------- | ------------------------ | -| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~/input/rois` | autoware_perception_msgs::TrafficLightRoiArray | traffic light detections | -| `~input/camera_info` | sensor_msgs::CameraInfo | target camera parameter | -| `~/input/cloud` | sensor_msgs::PointCloud2 | LiDAR point cloud | +| Name | Type | Description | +| ------------------------------------ | ------------------------------------------------ | -------------------------------- | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/car/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | vehicular traffic light signals | +| `~/input/pedestrian/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | pedestrian traffic light signals | +| `~/input/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | traffic light detections | +| `~/input/camera_info` | sensor_msgs::msg::CameraInfo | target camera parameter | +| `~/input/cloud` | sensor_msgs::msg::PointCloud2 | LiDAR point cloud | ## Output topics -| Name | Type | Description | -| -------------------- | ---------------------------------------------------- | ---------------------------- | -| `~/output/occlusion` | autoware_perception_msgs::TrafficLightOcclusionArray | occlusion ratios of each roi | +| Name | Type | Description | +| -------------------------- | --------------------------------------------- | -------------------------------------------------------------- | +| `~/output/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | traffic light signals which occluded image results overwritten | ## Node parameters @@ -34,3 +38,4 @@ If no point cloud is received or all point clouds have very large stamp differen | `max_valid_pt_dist` | double | The points within this distance would be used for calculation | | `max_image_cloud_delay` | double | The maximum delay between LiDAR point cloud and camera image | | `max_wait_t` | double | The maximum time waiting for the LiDAR point cloud | +| `max_occlusion_ratio` | int | The maximum occlusion ratio for setting signal as unknown | From ab6c3b5bd159a723f7ed6836671d370a30929226 Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Wed, 15 Jan 2025 18:04:27 +0900 Subject: [PATCH 24/59] chore(autoware_traffic_light_multi_camera_fusion): modify docs (#9821) * fix docs Signed-off-by: MasatoSaeki * add condition Signed-off-by: MasatoSaeki --------- Signed-off-by: MasatoSaeki --- .../README.md | 31 ++++++++++++------- 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/perception/autoware_traffic_light_multi_camera_fusion/README.md b/perception/autoware_traffic_light_multi_camera_fusion/README.md index f7ee294cda147..b54c623f5d750 100644 --- a/perception/autoware_traffic_light_multi_camera_fusion/README.md +++ b/perception/autoware_traffic_light_multi_camera_fusion/README.md @@ -1,29 +1,36 @@ -# The `traffic_light_multi_camera_fusion` Package +# autoware_traffic_light_multi_camera_fusion ## Overview -`traffic_light_multi_camera_fusion` performs traffic light signal fusion which can be summarized as the following two tasks: +`autoware_traffic_light_multi_camera_fusion` performs traffic light signal fusion which can be summarized as the following two tasks: -1. Multi-Camera-Fusion: performed on single traffic light signal detected by different cameras. -2. Group-Fusion: performed on traffic light signals within the same group, which means traffic lights sharing the same regulatory element id defined in lanelet2 map. +1. Multi-Camera-Fusion: fusion each traffic light signal detected by different cameras. +2. Group-Fusion: Fusion each traffic light signal within the same group, which means traffic lights share the same regulatory element ID defined in lanelet2 map. + +The fusion method is below. + +1. Use the results of the new timestamp if the results are from the same sensor +2. Use the results that are not `elements.size() == 1 && color == UNKNOWN && shape == UNKNOWN` +3. Use the results that each vertex of ROI is not at the edge of the image +4. Use the results of high confidence ## Input topics For every camera, the following three topics are subscribed: -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------- | --------------------------------------------------- | -| `~//camera_info` | sensor_msgs::CameraInfo | camera info from traffic_light_map_based_detector | -| `~//rois` | tier4_perception_msgs::TrafficLightRoiArray | detection roi from traffic_light_fine_detector | -| `~//traffic_signals` | tier4_perception_msgs::TrafficLightSignalArray | classification result from traffic_light_classifier | +| Name | Type | Description | +| ----------------------------------------------------- | ------------------------------------------------ | ------------------------------------- | +| `~//camera_info` | sensor_msgs::msg::CameraInfo | camera info from map_based_detector | +| `~//detection/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | detection roi from fine_detector | +| `~//classification/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | classification result from classifier | You don't need to configure these topics manually. Just provide the `camera_namespaces` parameter and the node will automatically extract the `` and create the subscribers. ## Output topics -| Name | Type | Description | -| -------------------------- | ------------------------------------------------- | ---------------------------------- | -| `~/output/traffic_signals` | autoware_perception_msgs::TrafficLightSignalArray | traffic light signal fusion result | +| Name | Type | Description | +| -------------------------- | ----------------------------------------------------- | ---------------------------------- | +| `~/output/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light signal fusion result | ## Node parameters From 65f6417db4bf4bda100eb961a841f6d31c94c05b Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Wed, 15 Jan 2025 18:04:41 +0900 Subject: [PATCH 25/59] chore(autoware_traffic_light_classifier): modify docs (#9819) * modify docs Signed-off-by: MasatoSaeki * style(pre-commit): autofix * fix docs Signed-off-by: MasatoSaeki --------- Signed-off-by: MasatoSaeki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 32 +++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/perception/autoware_traffic_light_classifier/README.md b/perception/autoware_traffic_light_classifier/README.md index 6e720aabc7593..7dcd4a73380bb 100644 --- a/perception/autoware_traffic_light_classifier/README.md +++ b/perception/autoware_traffic_light_classifier/README.md @@ -1,22 +1,33 @@ -# traffic_light_classifier +# autoware_traffic_light_classifier ## Purpose -traffic_light_classifier is a package for classifying traffic light labels using cropped image around a traffic light. This package has two classifier models: `cnn_classifier` and `hsv_classifier`. +`autoware_traffic_light_classifier` is a package for classifying traffic light labels using cropped image around a traffic light. This package has two classifier models: `cnn_classifier` and `hsv_classifier`. ## Inner-workings / Algorithms +If height and width of `~/input/rois` is `0`, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `CIRCLE`, and `0.0`. +If `~/input/rois` is judged as backlight, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `UNKNOWN`, and `0.0`. + ### cnn_classifier -Traffic light labels are classified by EfficientNet-b1 or MobileNet-v2. -Totally 83400 (58600 for training, 14800 for evaluation and 10000 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. -The information of the models is listed here: +Traffic light labels are classified by EfficientNet-b1 or MobileNet-v2. +We trained classifiers for vehicular signals and pedestrian signals separately. +For vehicular signals, a total of 83400 (58600 for training, 14800 for evaluation and 10000 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. | Name | Input Size | Test Accuracy | | --------------- | ---------- | ------------- | | EfficientNet-b1 | 128 x 128 | 99.76% | | MobileNet-v2 | 224 x 224 | 99.81% | +For pedestrian signals, a total of 21199 (17860 for training, 2114 for evaluation and 1225 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. +The information of the models is listed here: + +| Name | Input Size | Test Accuracy | +| --------------- | ---------- | ------------- | +| EfficientNet-b1 | 128 x 128 | 97.89% | +| MobileNet-v2 | 224 x 224 | 99.10% | + ### hsv_classifier Traffic light colors (green, yellow and red) are classified in HSV model. @@ -52,11 +63,12 @@ These colors and shapes are assigned to the message as follows: ### Node Parameters -| Name | Type | Description | -| --------------------- | ----- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `classifier_type` | int | if the value is `1`, cnn_classifier is used | -| `data_path` | str | packages data and artifacts directory path | -| `backlight_threshold` | float | If the intensity get grater than this overwrite with UNKNOWN in corresponding RoI. Note that, if the value is much higher, the node only overwrites in the harsher backlight situations. Therefore, If you wouldn't like to use this feature set this value to `1.0`. The value can be `[0.0, 1.0]`. The confidence of overwritten signal is set to `0.0`. | +| Name | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `classifier_type` | int | If the value is `1`, cnn_classifier is used | +| `data_path` | str | Packages data and artifacts directory path | +| `backlight_threshold` | double | If the intensity of light is grater than this threshold, the color and shape of the corresponding ROI will be overwritten with UNKNOWN, and the confidence of the overwritten signal will be set to `0.0`. The value should be set in the range of `[0.0, 1.0]`. If you wouldn't like to use this feature, please set it to `1.0`. | +| `classify_traffic_light_type` | int | If the value is `0`, vehicular signals are classified. If the value is `1`, pedestrian signals are classified. | ### Core Parameters From b69241f6659c042a05bc9362c3d254e25fbaa290 Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Wed, 15 Jan 2025 18:04:56 +0900 Subject: [PATCH 26/59] chore(autoware_crosswalk_traffic_light_estimator): fix docs (#9822) * fix docs Signed-off-by: MasatoSaeki * fix docs Signed-off-by: MasatoSaeki * add tlr output image Signed-off-by: MasatoSaeki * modify sentense Signed-off-by: MasatoSaeki * modify sentense Signed-off-by: MasatoSaeki * refactor readme Signed-off-by: MasatoSaeki * fix docs Signed-off-by: MasatoSaeki * fix Signed-off-by: MasatoSaeki * style(pre-commit): autofix --------- Signed-off-by: MasatoSaeki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 81 ++++++++++++++---- .../images/flashing_state.png | Bin 0 -> 24574 bytes .../images/traffic_light.png | Bin 0 -> 54297 bytes 3 files changed, 63 insertions(+), 18 deletions(-) create mode 100644 perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png create mode 100644 perception/autoware_crosswalk_traffic_light_estimator/images/traffic_light.png diff --git a/perception/autoware_crosswalk_traffic_light_estimator/README.md b/perception/autoware_crosswalk_traffic_light_estimator/README.md index b14fefbd43beb..1b24b115f5812 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/README.md +++ b/perception/autoware_crosswalk_traffic_light_estimator/README.md @@ -1,40 +1,85 @@ -# crosswalk_traffic_light_estimator +# autoware_crosswalk_traffic_light_estimator ## Purpose -`crosswalk_traffic_light_estimator` is a module that estimates pedestrian traffic signals from HDMap and detected vehicle traffic signals. +`autoware_crosswalk_traffic_light_estimator` estimates pedestrian traffic signals which can be summarized as the following two tasks: + +- Estimate pedestrian traffic signals that are not subject to be detected by perception pipeline. +- Estimate whether pedestrian traffic signals are flashing and modify the result. + +This module works without `~/input/route`, but its behavior is outputting the subscribed results as is. ## Inputs / Outputs ### Input -| Name | Type | Description | -| ------------------------------------ | ------------------------------------------------ | ------------------ | -| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | -| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route | -| `~/input/classified/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | classified signals | +| Name | Type | Description | +| ------------------------------------ | ----------------------------------------------------- | ------------------ | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/route` | autoware_planning_msgs::msg::LaneletRoute | optional: route | +| `~/input/classified/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | classified signals | ### Output -| Name | Type | Description | -| -------------------------- | ------------------------------------------------------- | --------------------------------------------------------- | -| `~/output/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | output that contains estimated pedestrian traffic signals | +| Name | Type | Description | +| -------------------------- | ----------------------------------------------------- | --------------------------------------------------------- | +| `~/output/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | output that contains estimated pedestrian traffic signals | ## Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | -| `last_detect_color_hold_time` | `double` | The time threshold to hold for last detect color. | `2.0` | +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_last_detect_color` | bool | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | true | +| `last_detect_color_hold_time` | double | The time threshold to hold for last detect color. The unit is second. | 2.0 | +| `last_colors_hold_time` | double | The time threshold to hold for history detected pedestrian traffic light color. The unit is second. | 1.0 | ## Inner-workings / Algorithms +When the pedestrian traffic signals **are detected** by perception pipeline + +- If estimates the pedestrian traffic signals are flashing, overwrite the results +- Prefer the output from perception pipeline, but overwrite it if the pedestrian traffic signals are invalid(`no detection`, `backlight`, or `occlusion`) + +When the pedestrian traffic signals **are NOT detected** by perception pipeline + +- Estimate the color of pedestrian traffic signals based on detected vehicle traffic signals, HDMap, and route + +### Estimate whether pedestrian traffic signals are flashing + +```plantumul +start +if (the pedestrian traffic light classification result exists)then + : update the flashing flag according to the classification result(in_signal) and last_signals + if (the traffic light is flashing?)then(yes) + : update the traffic light state + else(no) + : the traffic light state is the same with the classification result +if (the classification result not exists) + : the traffic light state is the same with the estimation + : output the current traffic light state +end +``` + +#### Update flashing flag + +
+ +
+ +#### Update traffic light status + +
+ +
+ +### Estimate the color of pedestrian traffic signals + ```plantuml start -:subscribe detected traffic signals & HDMap; +:subscribe detected traffic signals, HDMap, and route; :extract crosswalk lanelets from HDMap; -:extract road lanelets that conflicts crosswalk; +:extract road lanelets that conflicts crosswalk from route; :initialize non_red_lanelets(lanelet::ConstLanelets); if (Latest detection result is **GREEN** or **AMBER**?) then (yes) :push back non_red_lanelets; @@ -58,7 +103,7 @@ end If traffic between pedestrians and vehicles is controlled by traffic signals, the crosswalk traffic signal maybe **RED** in order to prevent pedestrian from crossing when the following conditions are satisfied. -### Situation1 +#### Situation1 - crosswalk conflicts **STRAIGHT** lanelet - the lanelet refers **GREEN** or **AMBER** traffic signal (The following pictures show only **GREEN** case) @@ -70,7 +115,7 @@ If traffic between pedestrians and vehicles is controlled by traffic signals, th -### Situation2 +#### Situation2 - crosswalk conflicts different turn direction lanelets (STRAIGHT and LEFT, LEFT and RIGHT, RIGHT and STRAIGHT) - the lanelets refer **GREEN** or **AMBER** traffic signal (The following pictures show only **GREEN** case) diff --git a/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png b/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png new file mode 100644 index 0000000000000000000000000000000000000000..7686f3842e75c92cfd27f9de08ef1ecf6eb3651f GIT binary patch literal 24574 zcmd43WmFtdw>C&ZfB?bW-Q696LvVL@cemg!!L^a#8r&hcySr;+jXTqM@4espW@gR5 znYE^Vbam}Ib!4A?PVN2dr#n(nUJ?Ng7Y+gf0zq0zOc?^=GXVU(<0~}yb0)Sr9DMrX zA|kE&6@2)9HH`pYzlPMH*n2k&=^$+o8Nu z`gs_lbZBi*%w$xD#JbCDw_?ty_x$4nk4jC7c2_K^F&xwO=1+M`&$mZXXp+Ofd%7lE z66Jjg`g^q|SOYw5bN2N21^8U}9I`$52|citc6D_Tl7t|Od?oQ--tI#D{`KF*E4$P# z&3_3)$pPSdA;<+9{~wFEJZnx*dPBxi9jl)?~=@vhx9(%{-=$ z3hh6)A=frMw&YAr3q@bt%ZJd2Sy*QB(P01eJtB~SkFQ?mAjg3|8j#rUI-nD{2bPDQ;ez+IyTNqovHi)AB zX%jlE>k`zUR(t>P8XUtGcmDncg6IFZ_rR|NZvNrj7olLPe2Z%|&@F7aMTN1`z+PQ- zKPn$tb_zV3)9|KF@Q(7xHkpx3IpeLq)&Nr2mjAeqgSUMhD+2Hob*7$ubo^ZvY21+O z<}1D$ln)I~5kULzRs}CX{5L{ympau~m7+cdJKKTqXYcmpN*nkZ;jf)x!B{)*UP4rK zx6qybgk)0bm*Wij0x3rZyLkt!X}XfvJvO{S;gDE#TJxe7z)GExIOBz{EmMs(WGLV< zi5Z;U^t%wNEYQufrh^F2(f5vb#02;|jy{(`s)gp8ggHtg0S1+-0nNz3l_jPBm`%-{ zY?%Hvfpg7)DbwPoBlR!iTat{ay7OCz&3zk>Z?Am!{(mc6MN)u`c27$_fy(R=YoX$ zh#8|(=#+al#rj%v8>;sXgR@1;p`=W_wuYNEAo;-P$c3zyDxhU^wwk`+kkYx3vPp zW|SSa{daM+2GEV^H$`->=F+b|#e9zZ`GRQTdU_|uF5AQBPzH|)S zAtscoer(G0u@%pjR24NxAUD=wSS!jAU*QT*mylCnY_9q*k3snfWQ*dL*uzk1T8Fb{ zwijuS0)Ad4{eK33%qEcLsjddHZj03u4MTbPYiS&=w-ZwoRk^{5m`l{(wT#l@)8aU# z$v1Ft7ei3f<59rY2QjHnd{G6Dk5f(Z?9ZS?mVdpb?N0C>m{J0q?l|nhU+R%fXGX74 zPEkqAZV|27nbg4xuba}}<~qR|78RX;(=L$c$*3v!{OGI4=RfLciZJ4m5(GX0hb$sj zs(M>?`orS(7suSM5jAR^&&A(6Eb$%Q<_rhpmYBs`dxdobE`;W`eJarQ^j2x%`39)e zF4C2@cIvEzcjR@&kT6lNKS-60t)Z>i827P$jtL7M7{vV*T5L0j~*Jz1eB}29Nm-v2CBic zem{RQt!${?JC-@jv-vSm<(y!NeJhY)}XaSniDk2S0TUMz-7VYAgO{ zT5JSTJr3HF_O=jRe$L%{XUONXeyl76@O-d`Y!9uM^1ng)s39ob3_5qC{WYs= zQcLg+jkS;{cv) zwGVZPMWQJ-pD4Z_r1*neiPzxD8ej_$%f7==Vdzr*QgOxgmhsK0If2=M`cz}#)vezV zbHYK2uyuL2IF~o2%E#tW-vge9X+7$4Vf@}m<6?yR;Hg`G-|@hbvSsn+sFzkA0e7-Z zjKBrAIXbf}kZX)bT{#$+`Fg1TaTa-3J&tVWbmO^Rmv~@sb2mEMTegqDZa~i?R7Jy{ z1el`J+U?~1eSL$oms7R&W2&*k^@zE&g5*Z1f}6vdoTzdhWqp~fo%&B=NWcrA)2}f( z@ZV01KkMSrXal`Big7JiL3}AofMAt?rUdJKt?$|{Y|Ig{Jq$VvSP5=%nn>a~x1`c7 zJyN#wTRUQ?G5+AXi@75h@Qp8i;X}b^Pi$`;w-~V7iu4=w{Y7@)6A~KRmg@SM+4)N} z(JXVrDQSD$*7hsGj^m1JU{Y8J2HJ9W|M^fM+E3vaH@i~kltzS)MeA(b3+=5N$bimi z&#FGjYxQ!#MlD&^-5_G89r=u0S_H0O%*#+;5iMWJc_q7Jb1ty45be85JG=*9@U`e( zT+zZA1I!jo!&0I=gll#G)5Bj>rT-X}lEC6#S*s8G*k##HN)6MWV<7xDH-g?b4+M*Q zp;+>9$vy-JM?%36zUtC_KmWjPU!M%Nrm<{ZkH|#$yM}h|X(2|Fvf$C*N0Hv=zBk=^ zt-Tq%f6c?yVd{ZAD()FF@}gI`0e(n_%aNi^k_Ryph}?ij*D~9-?|Zx-Tf;NE+V1&Fkj^#^I1zys<^`o&Pn~Jy9fUIvP9z-V#;;iWWGTplE)q$ z&z`c(BaeopNnI-``Hdl)ua8f61Z|%@g2x{5blpv0wx$=af0PI#0A2M@cD=LpsPTsM z@*npCu4;%Xe1$BTJzjmkdc+eAuH*ck$zilGQ%aJPjNh-);1DA!+f}&{zmBUodv{3L z_|OYev8mFy7O{|8t@YPFC7iE536Lq=e8^lS7oBlZKsdfd-c_ zOfhY=j+TBbDr@u&3aKmen_^d}dn2!zw&*dmN!j^Oxw)AKq0PU)V(MoKRSwhw%T#;Z zJaukZV0HvYBlF$dCzBz6>*y+EN&}ioSUS`ph?#AIk9^f;O$l) z8J;{aa8{SpEy8a7oOy(fIixs!dN-x8X-ZCdD}g_IK5#VlFD>B{{x!U9<19du7AOgkC_r&+364n)$=Z8W{ZxEE)|EYCA5aeAt>W^ zoI~|-Q&BdpU*;FNPGIrA>Fx}X?;rJjbqrMrcna)@o=|W)ys77ZeC6uD=w3Hp_mCwR z{BpV+@F(*SX|;7+62_*~6?fE6PxouroY@?~yHli!o8!>@FS$hd?^PsI&QL9%H6R#o zda|~*UnF>7YGO^1jL&ka$1`@n+&Pw=?*jD!WM3DBK|4PXIsKR_%7(d-z@BQ)`i0ZX`=CwHoWAQt+w)KFDa48zK_pnn9AA)K?_qad?u0h?jHzCcsxdp(jdV(c*IC46B*u@M(xgs5Rd+RC4+Rj2tPZ7^p0`O=jUoG= z2&KBoWAJ}$<^8g7_!mAv4(2^SKO=7RIGruCZ2DXZs8?w(jHt}3She?OL~j=m^b-8m z0Zvqu)Xh#(SXd0|X9Z|YJsdGAx#O&Fu*Y=HWcRkwsj4sbG03-Y%82>%Hv`l2WgX8S zwX-0Wf!rqbE>>5D{{BE@_KmJ_CCSm^tc3-q;&FM3;EX)H*@6yjd!JA?v$V2GNRQ@x zxy`Ii=g`p~j&7Y@x>3zKk+16+YF2xZ=aFK1|D5Zkk;zH(@pKk32?^eY0H#|kgkH6h zos0V>Cp(^3frwhdOkmonRqULhu8y1_uk!_-qq}i$3qZH9BO1ihTZXo@KYvv^E@;v{ zT*0(9+zwP1{8-ZQ*ERpqIb33}6u9>F0$A2q@}D(xZkksoRIV~I);?V3j%f(<{kPFY za35c49uoUxUk2YscHO1^A~v4u^3A2t`IFO zftG3*uhZh_v3OBG`?F-%a+!VROy4dU{N)CO5~Qv)pNpi^#pO@uv&ZWxr;%QjK{PiOZ3<5(l3Z|s6DXw#$K24jrQ=< zCA8bj``!nwr1S7{a&T+OSOJpBKIVg^_Mq;*avZBNek==tQTc1GwaG&(#X4!7}M0>d1tBC^_ZQ7N1HV-a3hBH{8sImfZx&?_#@;ekv|7b zBobQ6zCvLDtJyIH*B!rYO@|f|S$dH|-87+NhH+l-3AB&^+`|kQvi2g=b35!syp3h^)p`9v&VnTJQnAxI+#Yw2ec}(qo6kgGz+r|nnWvC<{)$K-x zn%am&L~F@ss}E%6${l9S><6?b_1OeXLs*yLs0e;h=en?HPC&tv7{%IV;|d$-#uZ=5 zw^c51|LxY4|3yLx-jWy1MH+f0zF2V^Q_S`j)p-4TZTj$Ls<5#U4P!BbnEURo_FyHU zywPM0o07#KY7%xdpBegvZ|?1ptR{1}BFo;Sj2nQt9u39_DU zPnX}wBR8Zqij~4RNvEp9V-gE}w`jDsIi-gW=gAaiz9mvUujbsoUju4qocZG?2mNO)q1$qDGyXGlqbTtD18AcmerXjSZ{;zW;5pof|8`3@A!x2U{sKuz<9dw9(Llxg&c(spsr{z~^ zlMr1R7*Y?BKP>qr2C}Z8rcgPWf`0pOq7oGK9CQRdh;qc@(WWCE7&FqDcvvya9ZI0S zBm|i$aE9$$$}bC9Y@uFGrbGuX5%*7gM^ymLDOnHr$q}w$>Au0m^}IwvBnq3@Um|p9X5=xC)s%d2$kpE+UFD}3i<7_>6fG1f_ zt0EGA23x!VDTLC9kvEE$sql_zpuTnAw|G0L1t7BmhtjO(}3IfdeUN+%&tqIeRc_JgG(e>pKsP;D;wZ|^m*+jq2;xZp# z=(p>yk)WAdFlbkz6W0B`4cq^{{avt$B|l`Uqf52ph$6|5?bluCkLd!_#1hU9f5gAl z%xC!wXFgmVAjxUgg28Z*Vfn8!Q3o{E_SlDM`@^?830r;GvoZOB8jt{~V4Ri0mdxv> z?zNrMG2h+TsRXb75%0YGDs>W~WJP8>!34H>e~`yA|&qC-By1h2*|8_T`d%U z*ATX^a9?X{z4wAF6#NLY%Yy4tOAi`sM?07m0MG4Ta?$hM5)-knEH|0=FUJ`>6)-+f zqfe8BV^9q0&RiA=KI@Cz_?2Xbea2Bj()N2YB0aYte{{RA(EyNrVx<-6MGjw>Onaqh zBb_6p!+iG-P4s|D<|Xf_UO0SX=|W>pM+@cu%!o~#`5S~6th3eFg^}ErtuGu3jGe|2u#3ighW+!4e~LLH zrVT9d66N@Q2A$N4q~l1&Fs0wX*M3@#9)25-y3bEo+?v93T12}x^{fYYTxPIcIaZ6; zZWyL7u7n?asEzM@G{1sj0SGMDi{B6!MK)@Aq4M)Q;j7YAJkE#aV52LWc8v^{qa|c8gNcVE5Vqz$IiRaN|qhp^!x_IMD36dwl5bF?ukbu z#1Vg-$BX%B3#~s3C5BUWk@6-DC$A#x-bNO&5Rf+&Lw>F>%bO|4EmdydN}{iavMEbJ z!-P-WQ%C1pN-N6ZYr{M^+WlrJ$+5LK0`)WOlL}ty9Nats#omeGHwHwl7dr-3Et)sX zmd?iL8B}%~=*;+=l>q$L4tN%V@&OAgbeBn-k;mVWL8sWg3YS#fsxreeh~Hv)t(+b# zL78j_ef{V23bFF>K9R6}?N8a z4oW?)0f{{U*v`4T_Li3R=4E&6%+xiWWlW#n*&)YFg+nS|CAodj>hGAtJ?uz%1n~o6 zG3FLsQIBA@Kg6K9bTMny;;15i=))O(>Jeiy5S_#-qSkg0!R(w^Y;bJY0L?k;asxkH zll?AB5|PiRO6xv5bCTCy?fAgTaBLVVIUxwjxGP^BxG&a3kgUVg6ImoSkq7;R!7}FF zXj#!Bi|o#pSe~P?U00JTD>qv``?9&2EvslVuv>Yuj{hCPTG1vD#rxGCManSxL!@9& z>Tv?UIL`LvYg@R;l?QL&2|S&3ZKSWbg5pmuL8OWACFs9CurOo*k!XwvoxgKRdIkbJ zzZY5ReR)R`OL&yPY#l;pN3%e_o26W2I$q4kS#oWebo1e}5_iak{M-8GF|^aPtg}Qc zCPXX%5aE_$s2Lz*PBaVrMGrYYN6D(eKw&n8%XtxNGPjPYl~hmIsUNKE`q_rVlk2>( zqREg4UiQ=!`JQy~l`)vK#8Ews%U!)F9%+zMM)2 z-VlGz`h1JJYQE|oNDmLJ#~%uo#hX7w`N{}!?h%cu=;HSTguOEJB^wnjT0H@$9R>?@I;v|+6|e!p?6n{;c- z;m(^oa$I6lf`{75{@zog6-kx zduE+bY@J`IVZLCoi8D<`=Ga8oGDVlQnM;pZVxVr}@B68`yH&jNXBisH3wV;ziW97) z-uF(Z(&>TyxgUeSn|E;Ty_25eaa}*Lh(UY*?5w4##W)QVqx6xd^!G{1DsI>eq4&-n zkyliz`h4^!TlqVl)Z^Kq!Cez%E56!Ev5_%?FxUuwUkqI-MGttF%XJW*j!a$1kw1h9 z$asqc`Q=e(HMUYYX5d70Lr+7a?86-9oS0&WOrK5TA;t0{Kw7suJig4s(w9SDYVAxU z%9qRS>-(WAH`P@JVkv z@p{z|wv=I$*3J$DML^j^;>5Gp3xtb9r0L!~068kQ07XSUC8cvMoy=`3?Fa*;Uj?;+!D zp!2=o4cdQnSl&r;#?QrFy)q`+ebYHb25dEhcm7~xO+-;_eg9$0pJBnFSfZ%3u#m() z6UnQu>K{Z$LZy0c0;TX2~0=uR0nfkQqKZ#ZCqurz&gf)9Q{#Q6ihvLJ(l zzYwxwMw7XASXsI$N-KZnPjir80lB^YM~=z}z%_lYxs|Mts&>up>8z+uJZWk9zN-oH zR0qE-M})kSeaFmtq%p2yRKdv~hxI*=@_?AjK9DJGzU^vvpd%NCjFD0rD%V>Of~^+H z7Z_Yj{f9dw%H1d1b*N=crzop%Vx zz*~Ox1`#9%ODg?g%>3y?$>#NpuZ~UP4ir>|^a z0wIEq29xwFDXtsim#=lRG_HTskc4=C!X+{ve+9|iBW4F~;;^$3QWfS%vRYdVARhsP z%p`H8>9$JpDwa4|;udU-(_IC_`z8G{_*c??Qq9%M`sMf$4dxI1P*UO&MC)L4s}4Rd zlrh`2)XB!~EB;vRt;?W&3Xn1qx2BZbFkXHRFa|k7v**YPw9jG^bV8Fr=Lz3|4}z7e z4F9-TTIyccB^OJxoR+^G zEO|rt^v51B60&l-$~JC8{q?9C<7jtQH{b?OKro;>?bY zDzYkD%l`zV!48asTd5GpJ-5wb@;!Kw_J1djEJ21SP4_VUyiCyj#~30F}kdfZ0*!3V7#^; zt>#P9nY+cFH{_t5&2V*?(jMhrT1)6b@-P5ea$?nYRxQ!@9eLAmaJ2rHCXcv{m%=Cr6T~XsD_G8Z!TJJyBj? z_r6hL09W{&bbamo4H^>MzNeQLi}^Sk1@1L# za=NF`rixSCPG*Z{gE_yCXY-)_j2C)O_t^9*S)!{319R^?d+|btNeY5HGm=DR;BtNh zah5~l8EiFv;RX{u`d$Yq`arSMIjDUGO!%6PYdG+uRNWtMmlP-XKM8%#SW+4FM!|g@ z867@lvredKvz2fg0TDQLC1 zmZ>g&y09J)2zciQ>*Bww2&%|^VBN4bba8Rns~sZT`z1-(W5|5_`1DKdSZ_5EtZnZf@4TQ};0tbe0qVyYn|>O4#8pEIt#8llP^)=kKY$;!e5 zYdGRtD?CX*#O-00!}Zfq?oA((P(%CqLZON`uLEPe;4}5nRPN;r$%CLYKRg`VzQ(NF z=-OJ=&C!%hO$iw2WR5EMW3$Al?|nkCSYx2zi6cGdemoHgzTK+O($PI#^&vUAx!Jk3 zBMQ>P{}c{}cGK|GTg+&ANeaZ}Y9RCpC`YryDK!&B-XmnUoctJdp%)!nY24HUMi;yVKn9I4f)Vz(aSM`(rrTTp%A~^pXaI{iQ$VFYe7R zo=eThXb#ru{QNw4&44!+d&+xl>k2jYJ<1&hX^U>MNDIkiT&e8bL4T-uQJV;V(#%`m z?;O84g)$2X%wtASzeM9g$ThOnqMidp?OE#Wt`P)~F{p29+MDbTN6tQkQ z>&L8(Fe?=}_#^0Tz7;eD1qENC7-A-@U&D1s%Es9&Eqiw-A%yv7?GOIwfUdfP zds&cw6kyeP+S2Avx?DOsQ8FeM8BNJ}`n7;($NehZe)Tt#9&@^9EhqjLIsryAb9V=6 zlIOoloW6CD!%m$6BO3$e^`Il6j|`@+$Dq|!ZEKqvJ#Nu(M6>BUVL$8~u(@SLsZV{| zl2*m1>pk7=VK)$JX!e1CYaQ#0csGF(yxEQL87GL28bBoQT3EWO+pSPqzmPNM*ZNyx(R>L z6p&eqhMEbG-d#ayBi_|lWpNnW3Gv`&DiJIARBHcN{~wSDvEH`4*a?Qt!0l?<+8AX)kT;Im8!`=xVVg-qwUDhE(%kTGe zY)i!I&cq$8A8?uD5lBcVJ_&rcg2q{^IzF^B(nz|TZ?X7{bNBs=7L0Rk$8oR+T{!)6 zD@?Qtbyl4^+6uQT5!yLqzI1wYT)`x~u!=o715db%O45pjZdm-`Dd;FKmo5BupF<3) zc(=sr;ENDTqNh9LgrAa_yjvwF#(!Z(3*E5R0A23^WVDX3M@R`-YwjJ$(mHE)T!~oR z0Tl%Sx2oow-GS%{Z@y2^NQAPgs_50#)f4(eU!~1-v z*h~34u8&VH${Wi?l}_`$+t(Z}mN*6|64jV1`*%GcKb!{_g0&q(qaA%3w{-~g{O4~r zv7StD0~XoP4J4IJ)Ll)niD{zMax#|M>tH^~PZa-tv6p(#fxsD~`~Xv4c(nt>ul{+NE{M&kT+}X?n4eS!&+D)>i84-TucDtiN};sFsi5l9ax5j z|27&_g?m*nQH1J;Nr6_+McCIC`lV(rFY8o6@oFzbQUXP=ktDWJyOMfd`HweaPk^Np z#6XmFP0@V$_rKq9yU%dIT8D{^4(40TTl`FR`}jtR`~Z9>l!~Z_QIT|~Y4h)yt2ZKj zs0nzp88>J9_j)l1cyludrwBpbE%|HTb7jj;=dLAu6dON9PWdEJ`)Y8jxx_^B`xN>m z4&LG|Hx9L(Dr*DAdE~9~XEp+*^QK&%!w9C}57^H>4BR#u{A3AbCfbC=3Ey?oUBiF3 z_D+bSP6147>_G58igtZ*wr3t4kk&W!w&?`&Bg@3vIJWfE(6RHyU6MLw1O){jA0HJ} zROY&m;J_*87MkUzl7u=SeiC49q@FA6ComdPSYXVT zJpmk3H(YK=JrfrmcY63P3WavmC3{+l>9VH^1;w!XiQTN4YQ*^m!J?+gDR135bpu)8 zjahbu{k5~(r{dn?FKHO6qqRX@EC+)qhWs;NQ2)ZA$=Ww zl=%ar03lOD+FBz%n|w=2+?iTD7CuNdIE<=`vLlcIR6fH5xs?<2AQ@&Qg}LlnqSTs^ zIG+h3Ni(F%u9W{M-oYNmxIv=u1qys>jVyK|7%u1wyQn?j2d-75tvi4>HKj>^u{RlR z?*TChXH+X6ZAOHxY>!vy%{YMAz$W|dQ5Z@Sap)^(Z1@LUffgZ8Z~T6IQxfkQ)La%k ztPkWT#5XpT+X*akM zcIis!jaZtLd{-RPqnfrvo7 z-SZWdO9!O?2e5%hHgmUocW)7V%Ga_v_TOY_%|>grGIy)Ty8mdGSl8T=cZ-Xr|cc4ZGm(PXglot1UdXOARe4JZ#V(F(>woM<24@$ z_ZVt~t@{6jo^kRB)TOC-FLmR#sMGaM;3z zho!>f1iq_e2^P*20LP~=s|YX42-e+05HD%2v!wp11x1@+Q-$Ca;I>%sVEuL?foS)D zMj?9sPi6l_Ie!juCJ&GCaG_RS-DVszG#9`6cvaEQ=xN)1z zWbVj{1ch;JBRIO#@r-~9j~+yzn)q6Bt$SK%AVs^X-t2s$e%C&r5!l4X0aK=W>Lrv@ z3r%P*dtAQGz$SV|2`b&MNGSVvqs*-cBKF%;@^OSF6C4U676#tU#TIrve+uP#PCT|? zF*uqhoHdZ^nSSV|^^S3~L1iLxq_UJi5)3V5Pivt1EiH7h{oZK)t)}?uc_;w5_=mS} zRb+DX>u&SC(d-%m?>u39o`xXI&=XtLsb6aK=)s}gFJoeG{8ymRBGiz+pqF@;-7=lr){K?;y(T&3qCrgdTnNEdbj!&2xymH@VZ?3ojgm0LfJUof~ovJF-ca zRw;UCbJ!cBl5Knf-Y4;;+ts7(WKCm8uEaXV=$60Sx1EkZNLEQj&kGbZ|0iueh!w@A z$t?sJuc_ScB2Y;GFy-&0L}TUx+;*zt=P+1~jS)NfV}E+^+~rwG+osx;UNd?EdES%G z9!Vm1R+m2cKV^_lAPsudQ6-gx*IvnyrFOU$B1-7g<*S z`&ZR~t{vskLZ}%3@t7%-0_96MN+e&9cwhJ8#2wui-jFuC=+M) znX*>RXoRKF!JAFP0u9{N+~4M76z3;jNdfX?ONW0!{~YGT2>%w)b1@8;?$!dy4~n$p zClvbiY2W8SXX4OfssicNO<%!EGaFKro4H}NhJw?VE^ zlbPWB`j<}+ob%0pN>qlec$OMFZ9d8VUsLg1*)uwJJ&%VA%YLx>2_loA)G?zq=IR(q zgLjw+*#-lkpK*gCGzDr)IJoh;A=P zzeJ@3b?C|`V;EH}qTOlI=eyGl#yd;-tjdOhT$D>Z>?5QEvU{@)ED9!LC@5JlJw~b5 zp9D{)GX5AEkMR>Y#-WAFrmth_ttqTz4QH1iXb!=kv{S?Rox{32L|RO8GH|X@`HCPEsrwY!C#v-mc{D6(9oeXBwhjP!UxSbVCfq&`pPL9xs z&d|!O&d~I=J5N>g1Q3zL_|y}-e$9`aYFpA1z;cJwvtAH&6lQnZ(rd=G-< zxn?H(HDs^(hjaMJ%523`a5OJRvDtHB$QYg2Y4uO02d2mdm3eQz0wk+(`tY9X7kv#N z%&4w1VUIK-e-AlRO`U}vdlE9vm}DmQNAaOL-&5*Izh47U%dDlpM|eXyk~9B>66kyr zf^R7*=CV(O{Ql=o3vjJ;eVTt(qWsnIr`~_W%VcD1=qN`BIwm9J+LG4A?ii)jCS7Hy zO6`1T;uPhEq2Dx>c8^*W+Cu3%0KmBlTj7@k`g!(ihZNJa;NlJkjrxShd-Q4 z?)Y}dVti>y>KC6tywG?eBtLr=Wv$dRF8%KsPz;Lp2 zB|PljZ&YN1IpX9m&y9N1BDc)a{Nh3dhP6X~Zn{-qtvf7CoQA1CA6nE6SyH-rAA!LM z_lurg{L$jd1u;&>ZM^@@gKzZ*8MbG{mO<0+nJjlX2$0w#Fj3KG_w-7lvCLktToXjB z{O{TE%TwcCkdv&>v;OYGVWLB{=;iV7`HHTypvslB?o=^W4W*b#Z)vFo6k;<=&vbI;J^PB_7- zbMq}5|JX|{d?%EX@r)#i(J^R|4{TAXVRGfg%wZiUWl6+@Z&I2z z756Re3NU4S$%?FGQVqhSNb(JC{Fy>vRryqa*^Tv3`{H#OW->U0T*n_1F%c!IAft3M zC$(xT{xR-;-|5DY9~?3)Ef!LI$Hesuq5$Gkx4HQ}t8Sful%8jNtZe`xx zk~5wl%T2<~6Q0oNqvNYQ#yT_I%B-jzLUCIXdCi?N-J2Y+^(qgloaaDC>9^jK-|fJS ze~_hKH3<7v>kcV?qf9{EDuLKE61Ny{b7^IThN2f_e{CDroE#~So6NQz{k z2P{3O8Mm|Wz5IFYj^cU!Nc~86{k$DC>4%0h3{$Y z*EzYX#w;u|X-K-4TzB2=_|-yi=jQ|8XuC1F!dGt+oGs{InHxW5SMv90(gio;3t&kZ zXKGe_ia~ngyF-oCPZIZQe|^-yZn~Mv8nDNi5l&Hck7t!yh)mNwrQ?QUr%XB17^3Fa zX9e6f89dk~YflDd0U}MbY49urNEE`jWNv+Z{ib=Al-2du%4|-!G3Gv@MNuYO*;Yk8m09+u1(|@`=9uT6;fjEwkjp>1b_lZI1TZi&-0oR4{!ZcyBB>oPFclQ;$ zwq?kkFq>1N@a}|e-2amLscSq1 zdl`(Cy+uHKcsyV-SxrMUx(c%WBOb)01zBx~1YNeqjvz@Oe^^rv)~o^&N&!uVjlrA1 z=XZ$9cFM-YN_Tp5aI(MM8@;dHs*pb3Vh7FWi)>eHgq(U_u?>*mzgP+l6g=btC_hWm zn2Z#tesxWRG`24hxw;Jf7AI+GX@Q_^oTwHaXz+%`kwh=8hB)G6&AX4YXf|#vXP$w{ z1-31*%eIZ-#abVT(J*r)Cq&YQ=4L2D0I?|LEiI7im?Nw*UG|CUz^IE?=r)0TgvE_X z;e0s60Wc)R2nvL0^OFmQfSPbOTeHoH3132mm?mwrpkfs&_TH3?zN z3hFd25#-9;gT(dbz-WWGAd-?Y>%iwU>`Ye#XW7|Vb7?gv+c%^W49+sIO7K}e>WJd# z4UNHEqBvgQ(ZRtL6&)GwlXnP!agdoktmE)zJVkIixK+`#zx1BO(}mn?eS z1WYkui}1EW17f)j{Y-<-rQoA^bmjwU^dnzDrN*;K>!1q%4TfjQcHd68uXgR>RNN5N zdNX1bo)qusqVL<$Pt~?`QBEj!Vx)5ayAqzD<((`_30Gmaf0X5vfzu!36!>KWhU!{w zxg4J5V5uM%!`Uxhz5UX}u3ZROw+*PxJE{-wj*+dW{id`(tN*>KT(}BK;u!QIP8`Va z4j&2rRS}m-$1_+~Mp@-FOVpq2Yl$2p@ zmA{4bQ$`pYJ3xWsss|(MgE3Zi^mWZ;)N0zVhZNgfOo6JZw&C?&ek*X_vKA9NsUuo2 z(YD#2Bad@>w2h%MAF4GKavjeuo)cyD_3cD*GGsRw%_h9rWDhao@Il@SaI)V>4hN8p z^}nFQw+2WVkMNS6WXbk-V7TtRoj0;~B0!1f-z4E>&LZFTuJunWptyPqA$2|R%5bh5 zA{yvq3rx4$fi!gGNrXsxeLQa#Pz7t*@h1t9b)@M{q+c0$KSNUX6pP#w zG1nW+pxs44+T`)t%*4(9Ts&`As2-xG=pMUqQbphfU z(k7cm0Hs6|rPMzdS}bRHuYU`_7AB{q?Hw*U&-yy>k~o8cAjCKo2HDJ z(0bp${R$*BbU0hEI@#n2@hnF_Fg3DXrO*b*9L8~4Fd8~ z-@i|+(RX7-Ei#h4RZJ3!OB&`O;SPvRPGTkzjm2W$uWsKHf5tm{voa4uV?V@i#ru|C zPKkoOe8q5n6OH4knzh{GEj0Rwe7FuA#_-B}nCJY4$HyL;@3Is$$Gm!Jwt&h#*1ePN zlSwq!RC2&Su7m(P^u#*J5!>Acp<*;QT1|4Ck7GIihAn7ad!^hMxg;v^w9*;5?_TJy zTeU>?3Y%FbE|Bb^=aB17jko7CJ&`?g-!WPy{N+?N%H7;RgNb3So<_0paVqwc9i23>hzK^ zreHcz5U;?Fi$z1{%{=jEH^a?_pu(zFbOO#_^^Ab0iui)_>JnjVU(HvCXizOy?V-m> zIvBlWm=HafiEf-n7&$TD3~BUTZ1-IdlKmjS|LJ9MPd7j9kXOMK zw~iH*yBK!Rf)KwLw!-I5GJcg6CediFrC+NP&=%q@mK8CXTgi_4EVe*E5b^x` zUjS>%K2_XC_hTn_I~M4K*x1gbpQ?#X76?*_N*Vi{c8S8AG$Acr%qt1{9?9+=2orIu znQE(@dz@@VBJMFc<)&6>D&cKOv(;nCM~j+$Wuz%DFnK>G_<}N?{N85|d8WK3$L5Zf zrq)id*;utHn6{#3v5c3RD1&k~TytT!=>h^d{cZ<1?be0W5XIgX#RoQ#H*)(U+@Z?? z4xuuIG-@@}5vq}y2|1wLe)Xi}a?SWsi>Y!cJu72ylAgT_L&=r2Me4<(@|^ZH7G*3$ zB3f4rWn97hS?;+f;!zZ`Dk$gxY!6`9Dzw3#JMCB>Q+k#=)_ZvJudti8SXsvGmLVv13UWWL7_-G$c(~j@(W&wul(g#vuj9l$sGiNg z+X_9yw}Tw$^1UASC^$zS2=nYGzM$+I!!!mo=io6B^?ww=3+0(|m-4oR``3gwWuzQm zkRDzbPTd$qJ#g0D{0vTFip>dS`?Mf0x`r~VfA92XQQVN)QOzZ4+5ecz6JD}PJ;E=R zpi$rp=S%5N*K^v??TmcPUL3;kQ|jJo)w#Pd>iwy_c7NQVf@+$+9K8J2PC%xMKq1B| z^$+xK3{6T8Q<74fJAFdgCDuJB6$7i=zfHuHqmLkb1n;+PYKC0O2@_Q-t9iBI`_O&v zl)SvR_`u;yyQS~%py{hokuvq{n&v+iKysv_qEK6>rn+-Wh^_FPminmwqnNAwin4px zA|N@0fJ%3FcL);FEs{z%(j`L*NGe@Jr!>+G9rMyCF(Wl7-HbFd3}@coS!bQU;HM?$F!BFh&8z1tbF141|CAjyQQz>HlTha^-A;qiGQj$^AP*+EAQV^rw{M{5FuJV6PZ6Nf_TqvtxaIdESmu6sm={W59EU!TF9Yuo_kGF^yfeK zr>4Z?ZH|A-?Atr#RZ0TA6!vMFt+5T~oA`2TIexiNMa(Tx1(j|W+@}&e+bJ?MHIjWM z&j8Uk);8*VpVS$<5o%!jUF{s+>9&eEI@!-*bQmpzS*sY*-(80iL3&thz32-e}n7#tjo(KPY=hOaCy z2W>>+eUJf(PK3XFJlof&U~XZ7Vc-9J<}A$d2mV1fd%^n}ud~0B4}GtTI?H>e@DUp9 z^mL>izptOGMA}7m*NOH+d}Bvyo>{MglJ%h~jVV0ZhE#c~E$9)HknyN*q`Asz-Brer z?-?r@{#kkv%3k*`#dX|#mEQS&Rgo@CNNQ!kcd&bTa&qz)1{r2^+rXJrlYH(jTp~gP zT$x3Syn?=S4w(L!6D zi0;QhR~?33Au^3JRJP17{Q7;rG}Q6vvV{Qlh~M%(YiVh5_VB0xgFnvvr6S^dmWV(j zPvPf`6hv3}IyySr2nZu51}bLx{NsMz*o8sri_yA-h7Mnaz1kIIF|yF?Q}l9-lS? z_KolD77HGnt^0xU$gAVci~aeZp||2tC=}9vztTUgCD+bmb<7ID%l?vl#)(_y~39puJU$=E94#2>D$RDH@yt zho~tGH(K{jNcJI;gXwE~Huwu!tuUe_J;nQrD}vWLI+-?l*SThV0ipa$b#;8E4VK(c z3;}?H0XAYQ{IB2yh+KhBq-kB3{QmSCb0~W?cF3>AmNkYY?rPMU)_2Q?h2O+x(q5xk zwBBJ@GYvM32I04_<*MRITx=P!944%*Vli-XoC_I1)4>HLq#fM z2R;c5x~&qWx~;v*GeLyG`3112z@pL5bS|vzQbm80RlFF4ZDUB{_USJL{BTuYbFhnJ zCwky6vCms3dPm5L?+Fnea#h=jQRaQ!|=3*=08ySU(6( zNRx5;>^&Nsl-$$P97cbncc}Dy%0=P z4~RaKRX9)-5>ls{>9_I=%!_>ZvtXfMKJ70hVU7oVbCmitXi+@~JgS(k4U;(CQR=0b zw4c4T5RUQ}_cJqh4i4a=O90tk*VgmFEkeR^3VYul*j%NfCWPB}wZqSYoG99oCAr-i zhZW#(5(HDBEh)3rXhCjlTHU5jjy`9|KaiQ~==AV6Vu#R&=@FCn*tamw~qB*BD&Z!G*%J-_b1JF}Lr=In=2$bi&ZhphmcTeaK zFOoYeIPmX7Bu3w?KR$IOz_f1qsyx-R!Yj3$uYiIj4kz!U%q(tGZKq(w0P-8hK;{;)^a?+;*FQrj%iDnCWvXI#s|Tj!JM z~y~~LL8E?gb`ldozuZ(25IWBt!`)^zk2g)xAT!O3y4Nd zjT!=MV}#fi3r5sVD)GDj{`s{8xtWN+5@?}+s>*hy|GRe**m?{fio+ZYbLqy>I!a{KGhojGJeN_`KTG z&^L=JpOy5y5^$pnZ9!&4-_PSaj;@M$QeSY^*xU2Z^+Y_m+wXTM<{KXqK^2}`+`mvV z&)S|zO=Az3kn^R#m{D05EmqhWxrZrdWARzJTV)(B&ln@4K^O%7Db4B^bJDoyyq z4vS^eKe_7JpRm!${Y0i7SCtW`p72r4Z8uN85rW?5YK)w@z_GYP$jer&Xx@j768rSj zxqk>SNp4VMTol#hKU3yo699(18Rx*>_4z%(d~1EoVyYuarNwDUpS(l`G-7O4(x|s4 zA`?Et=KEHlwur9R!#Nfv{HeF7NDVj%Hgdk$0UPAvYWc-SHV~cXB->8 z!OJD~BrH%$_fyTnI*z$~rpEP0i}?rpbAz38@rSRU>wqH2OMvN#c~@M_UIbl}SJ6# zzpy_}=jP8Zf5Ywkj|Y$w&*-=fNIdzAZ<0?47=6Z68AlaiI=cftnze`3NCWC6G`@~S zBBGVFF#4jxG%K7JH_cc(71ku5HYkbJAhBs~^Kuh6T5eYY+(&OrmG>k3hxR4uH-fQ9 zHmY+><8-Z(p-dJX?UJ_t(zc@#>scKrZCRq#i>k$;D%(RZr|ZW_nY{;Yq}wCz9ERqv z=lT;H-ex{czv}GSIR6op#&&D?biu|Xo?l?eyMq@QDb-B$eh67xgXRSI*HGUn888E< zR0bUxIUl?vR5Pu?jm8Ff*jDPUA3PAGPsG0Ef>+(O6C+3W`DWV?$4BbTZ>P&2edDIJ zXvjRmOaJYdbYi0xKYB_Rd^G1sSortH6)n-sb8t}j-bQ}*eNcjxn2pW{y3HZGbGfmv!f;jTHyfBqu)PoNl=Pcb1!tp3QCGexym1z_ryi%h_~L4DZj6H z?&ZMj$ggy1L#6NQITAQl?|Ja>Eh6E5t;v;W{S7#yK5FL4bKGs9?eL`~w9`)^ssCHd zG~|zcBX5;!I%q33?%)*fX$YY(w7k}euOQNMIm#dirnO%d&6q1G)%@TQGxviG0dV*X zu@n$7x^m<%X-)u;6n97SvEPw-Dkzo1 zV>>Qy;!(Wb0{ri{C&^{Hi3>hC(g_|b^&fHi@urFt1v>v6KboNTf@2Pj6#v85Gku=4 zdH7pnRzF{Pd&~DNKXQAhA?|@pCM>)_i%K>tSj7ciNO{FPy(HXpy%m?7k~h ziToO%Gl)d1w(k<_NYlg{_A9=Ut4;gv;SY=LnUnX-QatA>6n!tV={ZFz`wN}$2`!3M zN-9%*VM3y-dw4k?CN*_w;tB36`OOHjn^<^Wjz|`|D_fC9`U)Bz!?PPT^; zGDns>ks!yj^oD}PVd{x@DRw5^!y-sV)g~Ua&II)Z7XUmUGJeWyv(M}V1S-c@G+co6 z61b&`YvY5YMj!e@v$6og4n9VnwF7^<6FTU@J7kVW z0;dMPFxAgL+|QD{`!d^SsoZB+>V`@C*;O+d41{GYT<-0ol0_$+&j9{i{W3^_!0mqQ zi+M%e#9jLqEzMIoCMut2 z=jrzn=mXn4jHxqMdA<>vT+EQs`_4Is$-+-F{!CFfSF!QR0)e@S58zGRIRYh-%2vt& zrJ{|qs-YVPjO!0>sOsWzF-1h32uUmbZZ9ZUv@p04B>#Fr$mnkPcd_pmm$iTiMy)Sy zY19xS)iPkyThr~Yg$t<+MdpDwooU8tMwCgD9%2lKeQSi@@y77gb{93{GZAWGk^*jQVlIvaVIM;IUgl@WOSJpe*`9^XHye@NmUna zDKM57?$)kM+vSqj$A}@5?q-^@GEy7*+XwGLMS;Ghq8LUD%LPG6^r~vpEMAId$Bg#U zpB3%Hi<{)R{CLg1k+HvR5aoy6=0sHO30FPtrfQwK@JC?FNP5zIDKAB*=Gqs3$;wi= zD^ne3;}q>QWf^FD@o>q$=j6)ODOX7vtfW31;YRc2hS@!AM(Y-W~h6z3#xnvc2O~kJV7+uP0tMHD`t!VSh$EkNG%H4RaUI zI%>wNv3)Du^I3V>iDs;H+g^|!`CRmwmPpd^x^=n8l_?tmEfsnY%=s%z=4UHn0d?s8 zoUQIy;W@p=N|gq+y5|~%hE`+@=}kq4GB3((aSv9Xfa8%5B*_ju|v{#xL9ndy=zbIm|r`*wN{X}i+k}!Phs3% zF@yL%TK@6GQ{G#+PLse&AU#{v)_a;@oApFD&KM01%x#0Ib@L@lcf#fXdy0J6_3$_3 zVkRXWKDm^2BI_XBYm#e|vrj17ny_U)e<*<(M|e5(B})fWsMQat<78hZk0$m8#i|S$ zc#i2R7WSIHJ_OK&MSKCTfk~L!u9&^1e}YK#I9l&ZSDLhFm3M-Goa;v;J!!&MwXaKA zXn>1RvWGMCt5W27ADsu?CqNk~(mIioG|Snm)VjjFQy--Rm=BgDL8qbfun}mdYO1Cs!$dC4w$?7Bs>Vm-)nj2qqMsa7GHexp-dx!v24GKxue9N(g-EMEa zKCykP1$;81+X|QwV0bCplB@o3p~n>*3T9n&wti3?=Yz@H=5piCPly!U?hBoTCjEY4 z|3fkoEz9o^LfUMt-?v#T=C(C#TD3XFPW*!WBL`_ainz^Zrf-$w?|D5}NX!O87+V^t zj59Oxz6OLMuzvOhHs`IqE%R|nWlF!Pj+}_#MVrTfW>WYA8pGd}%E} zeWV!MmUx^Eq5G|_Ln(-)5Gf)^u`y*Hk!R;Gv_)HamL02L&#Q5&ZeEh^N^ytPk;C!o zaRlD{nFevGP&jyz1a!rX)6^Raf!SE9OUbmci`;rt8O%@L^eZ zlSEP6WFM2P4t%>R^T)0Bg6f~IO2v*ns;uTAwq&`O=lHn<<6d_jbv3{bysPo*Fw89? zk!&DC$Eh9Y?Q;!GKLn-^)}RQ#n~2plmE}Y}Q}3os8ax zi8_-%23+E7Iu>|&cGa@``M}w9aLafHPCC47ZLCu4z#Et3ypzxIIw_sm-krAuaIYTi zm;0Z3qHMU%DrTnP$={ES`T2*De!LB=vcE1OQmz}%HM*$TTRRGsig2&bRYe(x^0pnX z&pjv^&-CkhxNWkN|K{UIsxXH*qK-j(GJ5ZSppEfn{5h15d(Qa8VFu2_uL@2!FTu#K zQT{8-h(?FBWctQza$sd{!XMhsqINrUOWfJ)W53lL+r!OaPq1D`^X)&n5*|H~9Yc|C zyL6%b3!84Kb|JQs7Cu6oUo}&x1MM1s0EX~06d(M+O78(xtfkb-kr}Wo7H8tl(_vPVd z!)&g7#G-IyYHjl!hW&%LA`rqWqU8+N#10XlLpfz2*ZEE}mn4({u$JPJ*^ z435QG>6wom81dY!aAM8hEEce|Gu{!b=Oz(ow;0?FvqY_vO|RpF8e!gYin`0hl7M6b zxi+kCP&_X^k>`v;_@{&_lwY9R7uoy{_8J&mPA$=VxFO3`bDaTYJl}^sb!?qIfhPmXCiZrF3ufFR<>huppQ@dT4%*C z(Dpw?`EtfSyIcg(sTc^aF-$s=^;de|Kcn!J>?F1Oun#h(D2p4KE?s@ptyTw(SPSH~gpZY*i!!-lAXNkf)abZrqd@Y8NrGnL&{i z*2z#^E+|eafKps9+@x6s&>M?p5xeYC+O{wMFHf*+Z7%mIheK6-+UJrH*T2T$VC{mH zy(Z^72i#CRnZG2;rN+T^Th~xOn@v+X(PwxgTG6SJ7E>+9*uF>?Vy^YWq5`)I+`izV zR2pusD?8vh3#6!bp2zJY<)1C}Cmh-S3wMR{A}9WZQKKAu((|yTy?46_ghOuTH)mG6 zAkPyEa^1rDcQxtYeM*Of%STVL=UY}M3EY+AUA_)r?BY(|bd5?eUd>;8BZ4|rNHG`+ z1LsG_%U*Vy;2~?BZq&+eD`DJg&KyDZoV1J&|e}6jU*HF}JlWTe^NWPOaTAy`#(1=bdoI>UG z@L!N2O~B~@NKhwD8P)vdKWLn4BvvouyrF`AFCDqZW5V39uj+}c$e2fz)BOM2({2R( zpEoh>ds9UJ)=Dvg`Tvh0jl7d5&1Of|87lwQ-IY<1`92#p-*`wBZkXul>91Sl{v+i7 zqu|(P?gT!T?7KUtZQN<%`_HH{8=Kw#1@fB*P*7-+5~7MwP_Q&mP|$~n@W3bi1d}(wpEvd* zl1hlcKMzF15a9m=4qw$AzFQkRIP2LNL77-tTN*Li8`v2cS=s-zb~u4;7X*I9ZmOi_ z@WswZ&%xB%>VuN0r4jHU6cif^8~aXU4J+$zS_lizZesoiR*v0dKQ`{2kRUb=aA9mX z;oK<{)CVX@QDG&Qw8Is5EoJ5BmovP%$Pgk-fz#Sws6D7X6$&*m3>^=aikp1Moa|U@ z`{9W#hFS&^kQ{YN-W)aY!8gME{gz$OUfw=jqHu3G zH^Fx5Rm z-FUcRiu%7YfD|+|-alL)m0X-v?fy?M%j3rU&s%B`|BpSq`TxC#|9M$+5bSg@QN`rx z6Pk+TZcMl+u?o)*J>R8V;-sfPBO~b6VQFwZq9;wFlXu8JOIUUYXh=rey6#vp04KFK``l&@N$^<~+$suItEQRsaD64>`7WjC1b5db!?0pTPWvJ^rYd)? z>dLU9EMa_g#k;m%H+07&7@hw^#}ZQTscrP1dUH%`Dw5-nW3RU>pITsr(Oe%Eq)YF_ zdW=L2z{RN%TRnyv3mB|7UdGO~4R^k>3Duve_GPG|T6wX!Rw~V|HzaT|zp~hLK8d1% zQhEknkaWyZJc$YK9zPeZ9;KEZ(f^;{v$<`hB3Y?{ggfhC-}qH;GJe`l%2$qwQ`IV5 zaRyncVq0*oByq9QoKK{gQ#PQ$Li3*u)>w_GrN!GD{$PTj|H(5-WmVVQI`bj!py0!p zo>ZTi}z&zLUmW++CN>(MsZ^d*DI?&nxmA zb1{6M6EbYa-OiOdPHK@%v>LkcRv9e!a1Y}x&#;0xyqB2gS-2#5I8_Jk^_@##8oglWK5cX zzm9l;&u-yo>EDr|&;-4xhJ?hJciu6wv{ZRU$*c*QN=AEg86g{%w$*+$tzZ|3E`j(^ z_I5LoNF?sOhNgmBc7!6`?Ll>Qz;UIXMF>fI+(L^7lwZaS`|REM4lTHU4o@vpsWV%P zJwJ@Yvy~|M+FiSKHpTr~QmXwJs7mC-hj>f@)@9?eGhdN95XTfa7K@Q8L z(ImAd#>^El_MR|~))u1VCwF3}Cy%xFD0si6{|)J`G%RdafaLwchOSJ%(a9_Emq8nN6h6=^4hq2|xB$%gC1fg-kdF+%M{CYV zqlBzx^i;+^%nJ&4RQmgkJ?$7iqt@|idXUtJG6qXQkYlUpZSR=jGUZg3O%U=M=_CvP z%D1K!N6e#{Bs(_1)Mva6K(7cY#mZ?LF`C?G6Us)5!_Ra~(FA1}$DgMyz&O@igC?^xxQd5PI7`%k_PB(Pl0drz)8!Ogaco`+Crg`5H z_n&h*H}BHQ4JzxS7mtsG-gKpvE!$nnpiBEbe;altFbZf|WO0dSzbjpb*WJd(` zHw@T9BU>}t*A@gbL0M-`%ZWe!UR#(4Meg^jJuEhhG)j&Zdli2Go;QB`krCfG41B_29I%( zuPEY+$qfUrw*;ROB;Dz)Jp3DDb+C_3y1pkq^u!!{KLp;;AUiGXov&(FWVQ;c_~AsIc$D1T@KNuy|i_=J17d*7ZS7Y!~fULk-#%&f(|#FT4=M1KSvNaTG~KVtk#YVV}mN`COcN z!FADqv08t76#fhweXV64PEtB@gLuC_OTKI?eiGXjjU=eOCG5Kt0c!W1`$v^o2Rk1{ zdmbw1ONkY!x1>cH{V(U^b#v*txj)1O9XPl4X2**{kz?EF$<|gg2IOhymYa-1$0Ueh z+egWYb~WQU-w1eFcIbUsLjYCEa>b>i_aA9)zTMv4jZI4%F8W?5Aur#zus~n})|5?0 z2H~T^r=_Rwk7vVDh)2akC<8EVLX4Y0LqS=#QHNTo^3%O*A&Z8-;)$H7$5X#Cz24q( zued=sZvOmIm?M(o{M+WdCHY6S6pw3NLO_{1tbS?URnXAQebtYKp> z4(NN0Rj+~J@}BiQvvAFkc6EA}MvqSKNI5f<#p z;34jNX#$?z;^lm2hhAi_5k>@aK7QVr+qhaZL4gxpgKiy1_yWOuD9N-QczNg8h-=i$1!g! zbWBkFWow%=}P`(kdAzx|Fe;Ryq7^zo4}C` z12fh2_?2f~MpJiK5`T`|T$N3$T&c@BD<4sKSjheLVjca!EpMhLTKZLy_)Dn!((}^; zG$I<>40mY5_tDYOi1!@GCMG6rx0@)SEuilCNj~yDjl0H3hE&?FZOmwPR+?ylfr7m{ z=}6Ol-UYXX#8gw@Z~fniRjjbE(A5<2%L%rIvEMqgzpjtqa^w?Y;(mzro_YK(8>BuE zQNAeHJh+(G+(0A}Na_pnOq8Jai}E6NDPyO!&IC~J~Pfm6^5EouXocT{@2i|~DUGy#g%`d>BGbmDKptPfotY>V^QP8V;R zKvA%~EFk)BYaZ#JN+jQYfHCjJs$~{!9rIpO!h z$9@jPRWDsE=CK)D(&|1}b<&>@9-a*U)}s;?)!FPHVB0f;bwen)WL(dWhxu*AM19%5 zxG__WR&COl^}wCTQ3(!hee_RlMKhpr*uKiHCwWh=oqlH%L{;vkuRqd6uA&6K+?$9> zcsUxL4_f}Mq&b>Obg%~=mNMXB_DmPwgSbM?^U}SEcG@xc{x(iuxos$*1O3y7KBHM> z&pgqcs^_Uvdi1w@d$YgmGfuka7c;B1xKxn8n>h5^n!o)P%v|+}Kx_FhnRCSVWOf_N z+#V84HeEam@jxK>T5`FQ_=C-ETSDDna*&&kk6phT(dl@Zc1{Io`^drv2M6KT@IIMN zQj$|pKyC_h*x2moKUP+C%Ub&zSnhmtg}b{;7#}@2SQ{D|7-owbV_a30W&8+M?3ocq zn{%^DT;!5$NqFQ>7>2QNsOlNXhW0;b+AWp+MnEvrytGzcU;JY6MH?qRCPo$dn$v}Q z?XEjTLm|q{#BuA2uHWRKg@_Hl0gKcvPFC+=+$sG*e%T(EQKNe}nce8jfj&MpmEK%F z0ArIW=J)U3HqQsOnx7Ml(GUM=Wm9a0AB})Tm#rAgj6!q2kIa?YK(y-3L~I)d)?1mt zZE)f=aOEkD$A4w*XM0!SqY>VpEOYJX`ioN>Q=JdBRYybxnqC~!Z2FeKY43JOd#GA< z{6!$tF`g-T=@y^?azApM%E5VD%jDFQ(KXPAY9#M6%XJI%udkcAad1>^$HqD{}lD6b2ze})Vm^CV&!=<~aQ|1f& zMhDK8;#Ol!dz7L$L4Zu|JQUQj&MlZ z##Y`Cl|SkBY*)ykwm*iuH%d(h5jmAy4|}a5We!PBi>Q>gN)$MYs|b2E21-pT+TA{s ziM%yZofI?iU;9k6`?j`ltJWM|DPf-2)#AP_AeVZ4-{SOQ29tFXx_;;=H9SNsq3_O@ z7-AXR6vrg^3-Fw{QD{wd6E=={E0M zw3UwUGBPYSI>EoXqW#yoJ(|0V>0Hp9@EwA_#Y7lA=c(!TmfjeJSa1-6kotb(OoUKWL0^6x5rR zhEFA#-ZXOBwwr+2UnsDD6e+kJkn0VdpAoJkBQ}SNdgu(US044R#texWz**_Ym7*-g4c#bF~I(@<0gu=Q^MiqXq|W1e8hncNq4=$s1lX^+4aQ zzjQ+~f9aFe7jXzxe>rHx9S0#XkNB3yB)pE%|Qb!9RtBPM_71WiKY&pcJ(X7L#dEb=En$AcuwV0xFj2u;_;&T`D#lB6TuRF5m^s}a4W>tP1qv3O* z&z?1qB2k*bd>in8XzY?MZ#<-$c`-gMV+t6*C2bMi7V@TmCJ2_pu|Ll=8BhWdfTyBpXbn{DFzE$9c71yA(m7VC^$~cH;EWUe_+jcBnTvw^ z@}TrWa{NdhrL)I!k_9gYL9jmTNz{qSX3c&t#4LoW-MF;l(*B%+V{W&LA zDeu=0187e#Ia^M`c7Y?W4dU!Jl`r4mJ2Q*M&mQ^f3{E2skUoOyG}{w_+yHugDURQLmM9B3!wPal4K8K$l1hbU7$?)>~B9z z#a8O2tM9wtg?^WNZZC=A;zTv8mju*q4Q#{h75RF&C zxE8y?EfLft+I-n+4NR|ubx8RkMSTnFt4`O`Svx#PwT?q;eCRE19lo z;XBcm^e+|yq^UFOUd}Z%4kA>h58>SV2Y=I$j*11_0iEY5*HD<9&5S`C)qYZJzW4f% z6LeptfK9cpulX0c{L;REC89_8^I_kG*1=l*7hzudM{9q#L&Lq6tS*L9wM zeb>!NNzK=Sd~bwJeH#zv4($@OgNHuO)Kit7T=}<0JuR_zX2<4r&43%!Q{OsW+WAiA z9B@LUD!XHlH`|*eJd9nhrZP(r!)$1tU(z5tNz=gzBHiIuhZ)BnwJgwo{i33j6F;@* z9jrNqDbGwbIBAhHmb=Y%toqjDHdhNwZ!|3gE?r+Ry4>;VuxBCc2=ld+b~qH>N17mq zj=*L1IjSOKLtRD%Tv`A1X$`$!*@}S(iKla{^L_k>0GG|lICp5WRzwrmnNoQqAPL^! z#W^%#mFpeh+g2u_t-Z6Cm|=aaCy!;;V43fGzSp#KP;n?-$fHuwk!1C0jE_L^Q6_t- z-|ltyuae68R2VUAS5{-7DxF$a_gNrQslL*EE6ep_3e8xsJFUzoI zb`gnLKKJ5y(Vt&?=~QX_*1L11igFo@6uF2MO>XL~FzJZ*XZ#b*W9&{huDnfjsFzhH zYC_^cLJ9_Nyqmq+0_B?C|Dfu@(#*BRbgrt65I>hLld%#0>MSPr2?yd7;kEzS%bVmVBm(tSpLA)ZtHDNL{134GBmuvYoBnKXWq6SanEz1bkJA)Ub=!4b9WWOOTxBmEG{olk_he5xS_tM(68 zPuu>n?st^WYf~{H8BwB8W=@zvBVcv6OL9o`p4$FWKUmOYGm=?Js+qge6WE`s)t6$* zeVGx$V7dY3%FezqBvK(7or2i3?{_T^-)9fxo;2FNa&fS&PaS&B6nx_B7g2#{fKGyt_ z9e&j9mFLneJxjcd+=%ctCSzStez98qSn_0-I3MnS5|H|H8D-wNgaoQ1aQOu1Aw|KIDrRYHAfm7unQI3uzGhbF!=Ixk@@$ z`qfyp$z%LZp4O6~9*nijd->bo{VeK=$7J1I&);(QCf28i3Zx_ls|mp*q+=1aNQQIP z1j&H=Lw~w?dzMa&e`rC@I2Y6=P=-rJf?nTY=}wTWeMoRPJzkd{l4dzD>AiE-(Q7ZN z(th~Iy6X_khcy8rF-JP+8VODC=De_TfG+=vtL55gVB)S5HO^j_oe5R}&e&3spt|lI zi?_PojQOkz>b0}+Pz4q_IkWAy;mlU@N`?&;8*}R1^$w;jW%+>`Jqo|uK?tgC@dZU3 zp?|JCjo@=Qnzcua{B#|;)S-n-vA4L2hPL6Xj>5r(WlNk_eU-s-oaKC5eiB+{5oO{$ z>upCW>G7G8`18Fvr*u0zE94JSDZ}Z(lJ5;EqyFS?b7T3M`?ttw@9e2LeBZ=zQM1oI zF&EKNBm{`6iX|$&JR~b&wed>Kp3K?E;Ylc*emWeb#;T=?J*A*__)$)-pgKf5ELJCj zZhUV-_hoHy_>+*fz|wa~ZGzzjF6^uu(N5!lb$vOSGwW?Fe#50~=R+Mm3%{eTM+6kg zJ_wN#5nCT7Qlv)8JT9iptu{oDyW!b8kDr+F@O&K~oj}fK!ghi)GTc_yg($V!Zh=x} zxA{IYJ#2kF=N4JDGD>B%Ig>i9-y_}K3YESu^Wh0PEgQpPzo+ohS;<_f?!{{RYP7eMs?ODO zF(?J%t#{^)J(FkH4sHuyxH5f{kx0lw#bRy90+AKAcy_|^?Z$YXucky@8%8gI{pNER z*>S0EgTZHyl)?OikqvA2fyE2*fs<6{YnCJGn3MZ&d=K^09jD#PGC|>E+)h$-nt~zy zwpC)`?+G-r-jP5j@F}RK=F|1jQXjw=Te&XR4@lk$?#{K;J96OlOaP62yfMi;jrBjO zr*5B3Gdkb=9Yn4lMb=5T{+X(ePT``2*j(q!qr+Ww zHWsT>0TDZgB;}^A%>A1c*|%V6$mJ)xCAqf&_cfEm-Jm0andWZ&c zC)8&VQGXMk%ISX}YH2li?Q5hq;XS^B9%zbbpK7J^DM=Y~wy(uSxbm-XY1G(U>`Zca zcPck@$slq{3TCFEeXy3Uma*8`0vZr7ZcdoCHhPT{&E87m-mEN*-5EcsKPUENKJ<|? z9%pUYPF36E=aXU)u@}P+Paxdb)5w^{KqShP<>WnZXnhgseQhYESK;K=!yCEzJN6@I z4%~jVxx2X6k%M}w9?9?Se?$8hWGm^OnGB7({$jepM~wr+5Ru(lB9X4>OgjBAW^(4! z>1-a*L)6jG(WD^{o|GtHLi|fx#H*Fx6IX(&@pcrw?4QAqmxA|Wzl?%N+!BlNcoDGX zt;wX@!nQsOlY`Tq2rgMZQa#_+NA9k3HQUO|1wv=P;pMu`{f`t}-6&+Dt+nD{D6RfY ztql%lJdzI^S^LJ;#iP_6SE}Mc3Yi|o)l-@>zM;G2P z29{r^2E6;hn;{-dNrCQCr;isFon@{8btZom+};)ZvO&B@JNV(bl$hZK+wx7WAXd&B z1~$dqfD+L64c+TBMh-N8mufXb0|K7G+6RwimLOSmiA_)YkQA`@)>Q3#De#yd6{t?q=G|UnaQ`Z||C#*l2G^$~st3miS5Nk(ALTLCCLerPpnFoiV1b;N z;eWqjg(lTL@y4ml_3f*B*wzku0`I)3GhTu1GNgQp2%B6Wb_R>lOTSMv>vUNRDQ?(Y z{raOFOt6EUp{4{;4zJ7e?b5lyfOiXuk$Y&!~i>BXK!B{ zMV2x?5Jz)%c{$A+1Z+Tz8W`KjnTgrVyNZL*2Vj8WfZ?7Hm4T_LxXViikU{oB;4){L{YVZy?=f<;gF*|7X1%7_3IY~DEA+O zF{^>_I^wc{HQ?tA z@N%ERLK^F~JL=!Re>XHWnGs7)ckL|10<72b{am}09x#vuO8d7ZOI_ES| ze6-S>SX#=kJDd!w#F#e z$;ru~67woSKBnv~RMS*eR@RtK1^M_uwLKj3fhb;MmLvJA4Jn64M}vXaR(&I_uvlQr za6jgOfrY*EemQwD{U<>K<39;{0kP)mED6cT$ka3jUs|98f`Y1?Pjm$D_j2jw8L)xO zph0)%Z4D^J=4|sTaKzsWWU}qSqZ)mt-$-1yF|Q97zL=WQ`uh433AiaiBwkYq=%2jE z@cz(3LP8!M9yL*u(9{u4a786*DEy z?{PnF1kX1)(wC@!HUU0lmNyEBj<&?FB0JXt))WM|gUFXJwNZ9+du#1N?l;E(^`FRU z&}(xrP5U1`_`Dib^E^O}-(F711MFi*Z?71H+Zo5*-TnD2+MC5Y5#6v#!d17}d+RASt(631V&UjQ5K)}{i;*6C>_YWa*( z;60|JX*{JmttqY-yJy!+)_@f*_e_({v3Q=LH=v)3LHEyhoU7vLOR$=z4R?1$<5t-y z6Ny##euJ7ca53l*K`qk2vy?3gymB=-Sk4rLBW1a$<*s=)uE+g%dhdmvF0lbFdwg8s z@aU*EYRU|am^Z%F{l;#)9}|#3#>uJV0rdS755>3gGBWpRt%9pkuK|Nk_P-gZyHyKK z*~T|sz&$PO^cc?SuJp%A8KD;$U)mkXPN?g4VXE(516c$8J3KrC7W^cBS2pctrx{+k z67a#nAWf!+Ut%I2G&J<#O0z0NVv{N7SY2-0%b8jZtu%!LdhbwP!Uf;JjjrC!oL%9R zh*)gi(EAbyejw2?1rY=zJvD;BZx9BM&!h1N;2pr+L`zExK8vmoaFVpl?T>80CU(7_ z!QLSuA-c1r>HuxcsKYry+G|sG^U2Tc$HjRx1j(D@ZQ`jT14$Vyhwn~yj3FE|b;Qrr z+dka4j}DlZp7`(CQdKi9#MgYjNjZ5?NHUDA88bS5Gj<>{Ai7+# zA)0}Kbefs#0ZqIPX!9q(3GNV3-1T!IP+`1yD7`s7L`>zJH@MgkDs5FP&#Av!# zS69-5H<^|Y^dFPqH{Q5{?Ic|>EV#GJIVU}q|FX)N)8V}Dg2nEBVdgCFpn&Oc600f+ z*W*^SP?g<|lmjowdIXbE<=RRgOST~xVbk>pf;T03M-4r8mMA_#t$5QHlakPL%}#Ym zz!fu2Bb#>;(p&K%346DEfn%lmH}-N`O3SZGZ?$ON5AnYUvKxN&bbv@J9`D{vO0MT3 zxqa@rHGn}f;V&=4N)EcaT5$o%D#}0y@PfX(PeDk zF6joj1SV-&?|QUF!g^O|Xxrs} zc9srza8ZW5li8XI^c7SP4r^Wph$IaHqwctlcxFALk_2T2!&2}FW&+S6Wyr7K9O#;VWehKxh5xSqtprJwfZd)Ea7FQv$! zvu>}uG=0Mt@XCe8OYQUxxI65fb7dC#uiIx<+v_b)`tbGT{7g=3UKewt9!2a(b#Us= zgK>j_eaKCdq|CnmHnI>Jnl=uHg?kv968l+%%xADe04b^I4cm2$d5SrqM1O$!5UTV2 zU4#D}c>xNCh^DG1cqWcU!rD@v(%hKM9;i=ahK<>Qv<1{aldbN~psf&dAY-2eEw#M+ z3i&#klh2)K4nyy(b>ITdOb?fitVOp}D1Gl_by9?a7>d?cajp(ut~7pg9R0FB5s{4F zowUk@6mPU0fuz-Z)1ZeIw7CF#qfs?YRh{G9w_^wsR2!Ps&tkV*6NBBSC^@DgJn^L! zzG5NILi(>uiC^z1@r!NSYos<%u(7o*I*av#iC3WLUihe!R-Tj6p}j*5`DV42;no{6 zQOa_;p|LyD;&vD6Qgg)K|0v|1CB|Q<6F(ES zo62}gkRRj3T)cObsbLn;)WihgPyDjYlS9MaVb+Im2JCgMi<6wPe?BYm?c2B47Dm_~ z-jw+3bdGp5iO1beV$0>EG%5jGKmxt$CWQV*JsiHG&b+0LH(GC|jTCVObL3jUw%gOu zhp_~EzUmsyqtLs%5ar1QkCfh+F1w%os|I7*I;)FVzVLhUKzk6cNWt|TbpJ=JC|jH=spN6KSi zE8D#D>ad9@xn;hnS6ZFTYm~Yvr4{Z%m);Y2Pf&a1qH#${QBp*<>Td9Gm+d_;&%D3B zIp+SddM8rD4OI4dSy?_pPy6z~G*bO*O;l|sw2n-**wuoBFO+S%c_1t?in0xDlN*XVMj5Xu0XCCUN!Skt)3-7b= zYsCnuQ9Cl z-`6fK%de&Jg+6X+w&g+!J(8r|>*$6)&NA=%@MFGJC~AeuHf;*cXxN93R#r)-0IOmu z+rGiyIG_|LUTYY{qlm~@S!0084U`+gn*vl%>kmY1ghszc{Mx#h+Rd?~&jM!p?;z!#&t(e|m%&Q|R{i(B z5!iVp$QjTEW%M#1Wj-)fx=H1>2%^#LHM?&vT;RU&|0#c-X&3C99UT=8R?N`edY>W@ zBJ`J7TiHC26Kd4JW#=k%Z|uF=rH0Itv&zZQ*oRiB!P%dR=5M6dh(@55#b2PG%35}b z@k7joVF+ibZASA+{oP>p&PbI7eb5f%fjNyOst;4`Oy#qejywUXG%VhuwB% z!zp(aczffDC@oU@2r~&Lt_Y>q{cBt0$xeGz@pGj&BC{%o)#^yPf@6l=jrp0qj6{5k zp&dQ>4`nm!=!$}zSt$tzMmb|Me(LJ|z#{yQ9M-Js|`47HK3zU=v23)!rPbEVBU?1Uz;{Cjg8VZ|+m-LlMBPl8l;>)(S z`m_YBdYad+@#~+`(r#TA7PrfvByM|TfL7ouDk|2ky&nkUsAb7NeDIM?reG&SkIwimTHfQo$z#b}yAr1<)wBd;TG*LcbfN5g~#vAv~~3 zSTAkA@bJpP2w)7(XU2D`=AVmzolr$MucBr51u_of?{|Gczgy=r<=Jmdtbke}>lzNxGh^A8aIn?d zyj+OYwG#S0&%f^!zu&mIWy#P^h;l#W^SCeParvQX`tb>gs1)Jd{nK%ZBbDGY>H*Z= zBf)($pJrsc6+PxDDC=9B4g1{+16ot@FPEO9(Ta$+4GB)klu2Iu$JS1hl z4yM)AyWd#Z+1cf>n11!}^jrXbLmd|iG}$q)bC;A85trEHh)pL@6RDq?TE{ek-JPvl z+}GMzd@b|~1iHQNMoLyMHrcS7>#20B&I{yE;G$fzt;PaGV7xv-k|yxjpy zM9Q!0YG)+vl(PJc^?tfJhPv#4%+ENX4GBf)4Y{*wF<3`SJ6m_Elo=cwp7bsdeyx!+ z$x*K!Nicm$b9#c0%IPp10WRUO)2o){IiIY)dGqEc@z+nVaRc2b%q@R=!tw5w)}Di! z-CLcbU%`#U7>-xKqO*_GTc%JhJ&&a^c+Y|N`EJa6^W;SS^JgelR@VOvO?UnS_MCtV zcUyBmys3a8amYsz*lOdE&8M3+FaVeAj&Q9_zd|ML2 zY-r&A2WN~Yz2YmzuSxUq>B)6FM!Y79T;tc>1^8l=?*s!Av*Ubc2+%ozjD2~<2vT|q zGrgpmosRGYJsJSVLHT4h{qYq%QdGq9e%vA*7#f=5jRh2e_D>8u0I+!kaGNS9on^&D zChgn9>M@sfUj*P9=sKjlefIUIA;$mUQpx9Z18FjpOPORJpcp?_;lI(?505i%P|%;&UzcsTrv>U9HO)Y9ct>I&?Orulwm^>XOh2e#~lV}R78B}8r3(=b_zHPspsZXsKA_jKRffzZG z9@h&dAHzET-ESa2wv$d|+5x&>qCaSmnt=(3;{pC8NT)pd$F;x++L3j7BwH=j{pSe ze6QN$ijJv>0Opm~R}vNv>dix#uXS_w-Nt7zm`2 zlvnc*bG*Ma6@0!mD3D3^%jx&|@+m{7)veNGjHDC0KX z`5jk2`f3L90Re^B#Z2N6Z}YNCDaK|7)oXT|c!!ItqYkx}FO3IeCKa=uFdUg=wm<;= z1Y&Yc6or|A_5(V$)2vV9_B|d>bHNxzK!f=|Yx)F0k=EAMz@3nC$9EHhPXPR9I9I0a zaoT|}oXRB+;of9RM{z@7N$2q|+FctKlfzdYZ=TW#w*VaS^?q%LSf%9>Csc1VX}K+y zZMnJy<^X$i=q9Y&9LLix6s-mmfjA;`mB6={9Q?DG-A(`E{4@ z-<5Fy94xmrCJ%+!KIL}M1@ zFsj);@-DUx`|hKC4kt9Q-0$UCs(_y?z+%89VwcCWzS=1AH&8(EH+Z_g;=kLD1EOJt z=Ytc7Vr9x9UEu8a$kpJL3g9ARf0g6ze2AHrmR9dzx(KMwH{k(Vp~Ws$)y#~pyyZM> zHs%#>_kGQ82S=wHU59J!Kb(B<7#HK@1qU(&+_`Nx-*j|zqzSmI{KKQIUI9_j)<TCkO;W_ zBo+_hb>GiVR0FgC2xPcWWd!I5IEQ~{doZlaGG1%0n&GmE1W+d5tSQ7Ibb*D5_+5Sh zMdq*6)RKQFGsmkz{)q!1Og*+_m)&F&AZ&{IuQ;!Hii(R*0bM8niiAW*16~0HxPR8$ z06J-Z$+*3BS%B85c@MBj+cT;^cI3V9u}f6TXL;r5^JCj=N!Vxeb70bB|5i8&u=XjB z*ymem%5V3+sp61>`J2o=Tdpm?ED7?jAt|`s321*m#a34S9}c2EJ|Q8nL-)g{Pw@bw z^SVhfcCSa5*T}4;MKmUGLj+j4hPavX|1Da4`cmzDV*738j6OdguTse$zl|Hk7cNJJF5U5B>k)l8{+HGFuF zh@l}Rz%Fs{Dxz%k+^JFHI8(FlFK(rlk#t_tOxXmaqRGM=lOIbPjMn@tb?GAS{$H~I z8b=@_Gd$t*_~XM{ZC)!hMsp{nv=9l9784#2>GQl_p2fs5O%(uwN+y*vviZ0v{I$&# z>odK6gtH%5G^4tH{wY_6w#Eu0u1L;0_r6u51!?J*C{~}SE-QEFC!(Q#QT^hTVQocy zhj&Q7jtN2}B9W1i|NQy0z7b5y$r~n-3m6Cu149Hr5}h44pRif4@-wyF>gO78e8Jgf zvaJ@fz_QgVv{+4-oiw-$pk}1D{6+Biby&84^RvhM(CyggLjJlFfj7U8QEm8#56>{w z4r~(?&&+V;1)GD2Pu#f`R!AU@8=}#Y)UJ6)F4@}@FO`&4*I(I9zLD2$^_?by`_Kh<1K9$Aq1sm{G=u(fh9`(IX@7xTmRFhxo zV`>SO4R8wmu>1Vo8@G@6dR^yN$Ax+Lyz)k2Yd)W+0S((z$Inul_we_wrg*}K80=+4 zcz5WdWKAoLV-=r>>EuMW8BW?Bn1D!0&cm~?_mMpEpb1iDu+2MPO()-1f}YBzuT=tnGcri2{QZ+Sn_n4-QAQ5 zEv⁣FPPIvGG~FtIub<%&>U$#Ili*{16X&sCGqUPxEdgJ0zci(TDWpm^^IIGL7g- zF++TEopM=_kkz0M`zH6xp+>5tM_TB*zo13EH;&D2*p4J>+F?*CF{MldMGXl3M1`x& zh@F1Xe?~7G_GuhF9EH??@5vjK=vG94<-98rImngw1NqPdk@< z*4mCcm*&E0nckIxC-?4`ua}O@%#kG&8d(mwx&M01F4?SkhI;kmuD`fM3mqz z-=3Ucl03)3{(fB`B8rNMv3Wmx^hOezIOG|3wbzCad$CUl9Mmb4I7f%ZUlAsgI6ZE% z7STx7Q*Jncw~{R=ew`aW6xX`=!B@=l1kZ0d4-%g_;j_fWZff6(i`idO6=y!WR@LfmJ503H z@hJ=~Y`2K(n(aa2Xoakx&UjcD%C`n2nFY@Ei#-S}b)$$3iR%h&F)YlB^`=bWAxw;$ zzq{RizzR*`{dEghJwVhJvp**?P|e=&Q7BM3W@&g-*77%Gfx=|2By1TojySt>j?rhIF@B7>j_kK{H=kJcS*7VG8 zW>fL-@W6@c6psZ2(&+rk)oqqnQE5SvQ~NYGYSy-=>wO2*iundowz%=?WDUviWDm0# z&d?6$Fw8kK4>qKyrvi=QW4<{zGk10NJQ}VFQFhf*(7tx$XKwbZrk^W#yHbuc!z?d2 zl0ZRsv+g4s+K8l!A>){)^hME=t9MDH-aFO=Wk1L{)W5}|o2&BSyqjF-xAGW ztn7Vhw;iWJ(|ui1&S1~3)FO6#XIIPvT6*Kr=})WBi1A{}&gWfkT&nVi@R>ul{@JAW z8!3G$*Rhv(dwX7_w%5E-@|}rzFK@8QgRX-5SF4kbc^RF7;!=Qj>jgvF?%khSkGy1` z$!k0_ReSK4I!?m>x0Qi~$97e$3H-yb5QNS7Sb zTJb1SdbW0S_=KB%&p#I@Lg&MqYTdC-x!PQ6^r170y%@7Pyg@&27ZGot09uBO@TX`M zp{I!=5m^QSwNFOMbTX->G+1LEDLrfVHGe$%Y^7PV?a=|hb;4)l8NahJ~`sQbGCtt=epowg53{HD9in)w zQhHvAMu#RQ-X&rJs8gdK07i5!&W^{O4=0{Cf8cR<@19D@;%&-#yUT<(xR{r6H{ndV zD5N;YyFv`QdNkX7`@J!&J~>QwxcDwLhisM8Jdh?b)}F{6bHL+r!uxI})E2sMl``9b zxJy-Xi9$SYwafg`^*;yKG*E9OjNmG0e5P79*SPL%6d9LkCUmo^>y~1(elM#{LEqt^ ztik5d;CuJ6L;mpHmTeQzN~6~X5*78LaR?c&ZhsSKZvFo2Gq>zY3i}kf)NE(d&y(=5 zY-+|RpHiW~y~t8M6CYjKsT4kv&3YW6h7H@0*jO;mCjNo6y>#^S^xDN^QM@(} z%I!881Oz6QC1m6s^U(vz>kL|GD_xwlqS2{%g13V4CF|uHi`3&|1yP1(Kh9553C~7x z;fZ8gv(_I}{ye%Vlxe>m;kZpny%i(%Zloh#!LwTKRyI>i;CW~qQCP>#GZR#?Cu5ZX zl1tDMUno>%Ds|Vy$eg-M{QW(p#0QT83;Nlb9A2MpAIn6JT^%?L+Ruq{{C0HIkbkgt zc+3)2-9FVNa^&PT;AH%@iX8)Jp8J?==}oODvk%Wk#6Y>zEu2iureKyY!?M$zQFR>w zO5a?mx&nE2&8y1%P!uc3tM5=0v&Yu-E$qI{AfVE&ydPlEVBN*w=o1rK zl`E4cbqq)1{B1&30mI%(Q=zgu>*=Jz1l z?MhfXKHQ!Dq#N|h=5I2GNi;4n^F7P#MqS^#rQIu9MHf4GzC}-`=Inf&7U&5o^E^;! zSx3wQ8JJw~YEO%%JKAXsQ*m~UFl0}~GBLXAUSZ1Y>IBK%-8Oxg(uOz4)T!U5QJ(fd z-NuQ*|2)+6@sE4+7x=CBWXYQT9MeZS@<~MY5>_0~pMM}J-T3|MZY0X}=n-+3ou{)} z+@8*QG(L7-Iy+Qvzl`}jG;k(DNRGU=yhW4W4(u+$L6dfU1_#{EM>bjgUeP~S3YEgqjQ~*(Pr_x{g{h1 zC)xq8`r8v6$E}Hi&>vzf&FT9q4c4pT)l;PwX3oCL!dJ&-elyj^DPtCQ)s{Zl*4t0` zGt^Nd%EjmVP-0!ZUf`3!NQb7n`^VvHoPtZ-lSwSW+piQzcn8HA7xr7!|I!$h61TVf z%+TE1AbvjU>}=@TZXSzzY)_dx^l?>u&H3SxdGci}>X%ky?2?UA@)$4kv0^)qYmqyD zidp0QhthAl>d_jdP;9=Ew7eb1|8+*niDNQq#U+HI&uZ-cg9CiBmhMX2`Gtk;%1!5> zS`Rcb3bEvoDE^N-t<4oLUc5lEBmsf_?AAW|PlrSFDl|RqyfR-kkEu2@>8*Tf?Qr4@ zvv{ZwWP-`6OBPz!>+e%|X|g*1c3VoH)rBvj!a$+*C(6M+wDx$xnhtc?C#Wj%)-UvA zpQB6WYeFUx8S1zfd^~4_<*ICY^XRR>g z$$(G`%s;Jzs7_SZ4#ddx=pC#(42jbZC~Y4J|0MsF@x#k;eQ7u%YWjfTjL)i7Wirl3 zq%OAzrLXgT#fj{TOy*rvbrGMb7qcjVeUycHn9;Izou$ct)5I5LeI89vc7C@l>f6-2 zUdYJa;933K_xFd@i&oYsmZ8FIj;}>aqn$Dq4e=+-Lq7L$n)_K7W-y|_v(i2(RPw#$ zyA^fg$wid{)jli!_Sspb*ku!IDL$d#9m$E5GqYN}a3&OV&-SHW=w^vMQ68wK_7Qn% z@69vaQFMe_!OMze7DY^@pAnZyP9})k-sM|hM)v&i+s#_keTQPKpIu8yQ`ZDDemyQn zA8TP(3WyCb3nJdseqvXklc{}2*D6SqC_`>O?H2PSir|wsxi-8>i8uFlU`*wCcXsGE<7%XuOnp^PfJ9jxyPgH|-Bz3pdNE zbjRK)4GGB7|L{omNpbV|V9@4Na{`;`H5lB`(%!CnvbUn{wB7i;8P{--srt+^oJr%w z^XE;iVq|X|&63~Ei_Q`?gv2P+HC&IXk*Y8e_g4P^NV-L)TSV3 zCR5$epzPsj6YC&-b77~+jZO?_LFq`ov0v?~PgFC8nf^@Q*7_7O?rh5-m+{V-%rFm5 zYQRU8`5Lz_vALyahjZWXkrMgeFw-53osj~Lpp3^K^*VEdOWtEydG&9Nkx$PmzWmZL zK&d`A7nM7TK}2Tjs9bj7O?!Ox;UsEE>#Z%)(Q{N%X~9kIZ{|PBRZu=S%BHYkeC3t> zsG@;7;Bxa;){I}5sCg#i=CBZZm?360%tC=WlsPxK_3G>T{s0d=P2ra_`eXZ5R{IJK5({10RhF%hI=5hy9O*6w zLs3F>+I7#O{h5y(#0H;mPLD$S^p}l~)#{G7rq!lX&WC@}*|PBZU4PLisS!1%i+wGj z`%8iwWn0;k-_A$w8x3?LK9Vo4x}wq>u|)>Q${aW``go)y>_5uf|L`z{mh#16&C1lE zW`!e)dh|hw??PH~)}-7Jdb-NF{q;rsgv3?6-lgMDH3vo%m61ayai@6`{XKWg$K3jD z+C-c@H4WW$m^Fol<<2*F)N4PrsU2J{P&U5|-LyN34QO@`gBA-{9URq=EP5(s*R1p zUF9(JLZqhEE0fmvBqR2meW#u$?4y4a3P|w;|6J;^@j3-cbFRYzW)6GW^B9L&cFh?;ozk(7Nl1j}mBb%-^~3vA@Y3;1IuS zg7RwVK2GLd!0P9;MDL11!iEOdH`|ICHVX6ErSJD$7Z;w|Jn4xGa7|wO8KFsY@1A#L zBuP?|cY%r!pmSBLoOmFA#z<@e$=&LnT+FxB2c;$rE)g1$G?5q28b05$&O*_qV2wi+ zN#Ux?jrLe{7VHoq$}#coxu0AG`%`?W>~K&pJ(c74tv*&@F_HSSC_Z0{yW>IwBuLNs+Ts)ZgVC zu}y5gUGy`2h*?auHb56tuyZ(ga&XZ(nEB@8;e;K>70u+}ka{563 z=i+#{hC58Gyia$vX>{IRC(>eh<4fka=N1(?#TUia(Gi&Xjl^CG$6=iHYqCM5bxcm= zPsYLNU({Cis~-mWNyWbeS$(t|#AF>jSw4<=*YQ{P_q+9mkKg>Ad>iNLj~@ty%~Y24 zYdcMkaB#Y5UeukJBPzsK9{vO_2l3zCXig%?t(9i&|Sh6ERn^ zPm`XCCCBUN=s?sxT{xsU2i1=ysKRpbe)EUM0a?z;|mirCSjf~ckevCRb5%^xA%)2rE{I^h)$>+Y@fmGr~>PmGApmg07{^xDCAyb#*p8_DS(3{m2Y_=Fo}ZSTM*H5Z_oZ~!Q7@Gd=m z{kjJ#Sy$o)?kB)}a2gNSKsK=Raq+m0H92nZ^KBRe6Bd*=st! zceiUb$<@yJ|Bl`e;wrF(K-YGCC_eSc)#13>8YUtAqi=nTCHtGU`TE6FQaGoEYYEpI zoF0A26eDCXw`{9_GRA{}&n>txQ@C_bdP`xw?Pqenhh%-ged+1pD@SCeCBmP5ylbJT zAIW3Ikn}u2r{{+lm<(LQZ|sY7PX}4#zDBb7chJ)}bp5&cGks=eal?vtoJnMX91?!F;Uqt@vzse^y`M9l}h zkv0}E^LE|e#`e|Zd6`;Nls>zv92;3Dc|3OK+0wgjZ??!+9-c*ZD3#&7&^@mA@e2I( z{`ic*;GI10GWsAUn|^O2lv*bp)*fTI5gY21*>1G8Ggo(j6_D*(wAtcc9MV@6^xvvCDM}h)DySckxt2$mJ zgJR-vWjSeAGZT&Fr(fK-hQfo{THQ1CU*KHWczwOEPyg^^nuT7C8OXgeJb2(stZ84n zECEk`2~U2uK3*EaHEHkR>3JP_vH(2;!>nz2Yexr^$SGN1p%O{R3a+h>72~A#Nj`ga zZDXvM3x0P(6L37x+gThubkM44fo1@?e zojrj($0AqtalpUJLmgyAqjRA&{@~%m($l}?kZPpCqNAa4hw~6Tp2vsE2~%5pdq5x^ zP48PMbg(Nx!UUr3S#xfvsL)_#K>hq|f!TFt@QoQg90$a2caUWZFXu7^>;L-k<61;} z<8ZF-aZiwo9!QlT9kK-5@;GSC$OCvv=Q2Ib<3MB))NrBORg{t$FEZrG0gvEmb#>1W z_W<++57f?)1v8NlGSYN*0Eoh^QHT1$gEmO)>3jtM8&dSFtE+>KUV!%1AQU0?grSo9 z$eF@v%gsrPUUw6x{OAd}>qF_tC!c_+kb|7(LkJ>Wad~l?p;fK&5IRGiX=_sh zK!g`2)-eNynHAAkm1#hV8`zZm-hC?#Gv>GN-1&g)NQc*EwPEVwfD8T=1pojvv#|WT z9HHC|pafJs=bFgO*ntx4+Cf4W+(G<1H41u%#N6}v|wZ<49v_pfPaB{9KvLeJ0M|W!A%ZD z*%`}HNX5}u5&>ytTFA^QgbfYTh|~yzZ&+v{t=8FS*bUO%NFzz z5kd{3RgV=;vEVJ&loJ~&Dk^x`#N!st6rtha2rFJk1e1IbEz4JEu0hs)+B0u-gGA3yE{ z-s<&DTM}(}3nf^@hGCL%b|9C&n6PYr46>1DHYNDnpQIMrQ*^B9a1DRLNs+t<#7+gE zW7Mu{ieesmP@TC!?@>I&W&tfN662=Dz}(4%@)p3%J3E%ZGys7qFdFJYl-3pLINq?O zc*nrdPy(24hL9`MAIh63N9QiFvW$o0Ryc8dcARhqErx({kOS7Ga@x8IRb?&&oq))0 zCddLV3g|uHrY2^WZ46*B@BU@qhgmfXmSBHzK!uDH7>4{?1#)05;HNYLz^D^C3K4Qy zY68Rvv6+Cg2?-9?MmS>z;ea32m*<hkCF@%up86u8v$vBOt{g|teCP=8$;+DS6syg{FZpx*sZh;yc91IKpN8fA6#ZNaY zcCoW752A0>_(}MM#m9Gv;0QpoAgxM;eJ=9)C`5HUJ=mO*la8@2C?Okh8->`X)A!d5ayDJ1@Xpc z07j~IbzQeDN8TSDibiSg23T>VJ}oaVkK8X4iCj0G?Rx$YIu4-(Q-jb;Mm|B%dAQ;u z8zCVfNRmG&$YF$D*FoDJ2O`~;V1GEeL`CQMM+=Rbt-1xoL1Y1=4jpZy$b}6+t7B}z zvn>&zBO*42eNBkibk>F5S2)^Dxa`mk5_qnr7LA+%I-G&L9BA~q73*(bzt#k~p6P;5 z;m!UKv~*HV&eyy=*4f!v1aXFFTNyW4vV$dW+_=M9CQOB_U(UZb+V(#z0Ia@K9$E!~ z5J?8z@t}JVWkm**#w60Sz7o8654gLXsobCMZ%I9}*Um|QQns5I7`jk`TA!?pf&NuQ zMPdGk#}B?@GLrWh-j0WdhuFVf9n3rI-e3R`Hwk__0&185Td*K2x&q!oF0?z5W1|TR zui6vfmVO14UyCo{!{yP!hj2C(P}%{N7W_N~WGN7tMG8Wq+vGn3iZl)3P_+!eFzq(r z%b8!EALhTUy@ohew4i_F#hD`fwzkac>g)e8+$_?}2fPKQNLojS1_2TxN&*)SJ4@Fj|`4RRnW~MaH>>pwq|!dZr4JdK#VNoPRWGn@APz179CH7o0D$^dPSbnNRrfc(zISKN>t{E`&$-qXds~>(OB_WX>A|X?V0aOT$h)7U>k3fp5 z5E9JuGnFmV5Is_<0Vp3>%h^U^Eq1W1PG}&{fpFD@IH(m13k&O&-bRF>@!YoOn)9KL z7P$wc$OxgC0Eh`s*NSy70OS7a(87;kz5F_3xD-@XC8eb?shszMfoOD3;lXauF>ZCV zrLCi@i_n*5sum*fwuFm4#v5370O(A|T)FoRx%kQh+2nz-QY)l?J4MNj7+hfn+1Qr6 zckT8&t&AT5)`b`%8&KA2h2R9nslU&Ke-KR_jImraW|{;Y7GR_iHwNjrMf6{nNTW}I z+LY5S7Pt-HMZWX`WC{^J36vuMqqe{njvJX>gh92f4Q8^zQN)UT?YS!IKWkZQEyF(TteW4sR0Dc6`;&IqB+_@56GkJ$4aDw@H1S1%$vi>|0D4oNuVv5B*gZD z@OinOuW2EY!S;(gSjhk!6yPjX@cO(9o>A~xPXi6+Pu8+iT`wcj@RF5Mt z-;fY-20^Gu+4TMYU5xGkG>GCbB0?Mh0DprMB#{o5R;}W2$~6JVY=ml$i_4P=B8CJ( zr4up7*hC|6;daa^S2MJ8T0p*MdfV*@#V*r_$TvCE);Y?q6a}PKf z#S^w<(2NJj;*lAbjIgsfiwbsoW~ z0w)IXy)xb5iNT1iKlnS5ZUI(%CvJG-AOwTs`Sa&sD0Iweg2QECdE7sL{zT3Ka$-zn z5>ytrxW|9vT}}*5*%iGAd3xQ)!T&+LVlh9W z?R2g4Hu&}NH+c6e*p2ha-VDT9rT@+m^%f3JYEI7WgzjeWO}Fj&fjtag0U`Ym73=Qd z0ZvWMf39SUl76ICfF!HH6#~#QLP$Sk+aJ!i*-|5&A6nBKWr4OP8k_k(pb>Kc^NT@E4&;U4Cv$hX>m#hlTMdxUQmWxmO zf(~@Ks$?qtla+^hoH43-dg!>gxL|M~x&#H&Ym7=t@pQhtrM2}t_~-xiUBP(;2qV^i zoY#>`M=k^qfn2@*Ye_UZ6Vz!>iVPX}z#kA=;Z6@f3y9$|TLkZ74n_uOgUf-Ogc7oD zfS7XHZj64epLv3CK#O5k$t)reCEHD8z%6t_LQ}vkonCOhwqUO1m4@VSf*(5(CQv$a5dzDp3wyKvJ3jL7+9m6yI6DO95@S}?%Itb9g~&3f zj$}#XG<^G3<^jEx1ZZ@`dBaT$0lk%)psz|Iz-^ZZ0e>r+9XvfnWeo(6`cE-i)=q%0LqI@~ z1N;II>N*H-bin}-fz2@()q7z%foliD#*qjZ0`WyJGSj<4LJ5H30N(rv#|S5(1JQCX zT_vbK0zyJVL>(A%EOy2;msre!!2uTt>~6ztQ1q=Z1NpL*a2lH4EfFsBW{=jo*oi>A zQY7&B=ckA#_aLusUa>}U4q254YKYDEW-p5|E%N~n;R23?_5Gi_h$jXXm;DaBJGn%i zDXU`V1%xGL|Cp>^-4Z6$>g%&Q9ht+A-Qcny6uSpZ7V_%l%Nk%&W*E<5f?OQTFmrja zc?p#hS_nMAUm*jM)go)qhM6YwOG`!&{d%{Mx@`Mt<$T=L2S>ac5WyqmcG+OS?m&pd zGLAeCC;+7gb&Ezq#5&;`S~8Wdv*zXJufXsK@cAL_)CA0nMT3NY;2?kt4v7TB2czjt zmH~`8#Iz|gG0!C>eJJ<85s3>h>ij~4`(ajz`||Sp{~Q8F@stZ6c+wDb;!#m`!}M#| zmSrt?N%e>?hQvN#eVYE!s@ar(p&Jr^D98!CvnCJTly3B4FBAEKoeL8_fc21J`-nObjV=Ul(*wzztPj^h z<|Qp71H1|(c@g})7_L1z*k}*nntcE7go$PV0z?PXGnh2WZUY(;nEeCT2M6$y10I0$ zN_94khQ!kU1jx`rUP0lw)6AtFR75+BjOji(M6xJfUnKtp`XvDZ5Cd^F5C_kjJyE;f z3n=;&qM*6vpd}#86(mta4HH?wK21$~(iDPB2mHv$@UN)g2Zab{ibPf5n-;(_q6gBI z0d|HA_&_|fNI_>_&}{F(aem&6pB4uC!(7X08x`mUxi*)bkOUHNAw`o1;8Pd|ZK3x& z;ss(L#d2+W+a3vluw)tJ*?=FX!4007nHhK5?flP{Y+!yz14x$D&ZV;;6-4+5rEa9) zrA5ITUb>!+P2Ie8O9%3C|GgZB(}=!$g{!Ejm}+tfjzW~6bD40O=m)UocRG;VK6tp` zCH&Kp66w~Uf7qba1*VmhuIIIv%zu-Ty%>YhXsw-{98cm}M&Q^yhRf}k!J7SFj~86t z`;dV{j0e)B08QKB^YeOT)+_FBEa;Y0Wmg(u`^W8tVs2*p1@y5`Dq?@x?MhyC9Q zx!*!UJSI$2K_z2R$RPyyosppjnIx)Rh7fOegWeH1cMu$S!27*U`Y*X2d zEU_daMn;$5JrqMh+3`;+eW^I$_n3?oZ7(MU?fenJffj+ft~kEqX%8F;DJjI(RGc4- z0a}7+!#!iqjK0^ZG_rNx3ZSAjpZri zbdZ`NG+l61v;`TANT)nvs>N}OIBQhZBeo~IZ?oZKwh&x+FGwe$NAgNF+TrY@JS~ok z3U)7F6Q(?(^7}kaPW6IZX(3DU$;T`Z*nm^J()-izZ&C3TMlUXiKWDb>{R8`w*Z|x6hB@~@QtCl{fHgK?y3pOq$qNHWVJ{M+F;6JrNFExaM>LbI7 zKZV=~;8RXdzMy-V?x`YrRtE_Ec31>>QfixHk?}gera}vPtv`EuzSPiwRyZds%Qw)|0yKi= zH7L68D5yDF2!=~G#`#5AZ*0p5+s;xZh;+*yJnz&9`7_4I(u^s3X<>liIoCxE?<==$N^BAKoq4?2oGw!Hc?h%RV?4_Ra2?fKrXr zpH>$;H!iR`i$9?wHP77a__mp%W7!mCb3M*zi=5RARsL6=`bSp{4xpWRSC#lLMfsM} z(RYn|MKM1lBg6fZzVNu&E&P1ns%CrI-IM&UsGoDazeqp+or1*U<+g_V4C;Ms_jYU& zhdeM;qV>?6Lg)3K*cl3r9a3J&5BqyQ8+o)PO4GIM^Lsd4riq{Um|nAr)y|-XnTnyT ziB`>Cbml91G8C&ki?g~R=jA&K!? zUA+T3YGfcIbaZi8t>swnD^pT@l*1rqb#fm6Kj_@tf`}#pK!F+~P5TKAr)AHP5b?7#(x|+WC9;GK_-! z-hQ4?OW=UIu;NR!8u5$l8?3fI#73`a<~ED)oDw}bq!fIZha2nm{d@#86OTBhwU;ZI zD&l@f#x5s{D54wX&03sZ<&CwGwwikHP-HWwsVMy-Xsaxa81&K$zt_daj47lC_WY|9 zNuT`iW*ItBDNbjkAP?HC`-UQp%e&w~0 z5jmH^XTHDdBrURZPJAr5r0I%w%^fa(d&}^hHDIUB9r-4{k9W{z&JXIJq|o_b)I@&$ zw#1kf_j;{epS&+oW9+*2wpmnb2fb#-;bVijO{i)1_LZ zt!B_|a;(p<=>$%tM{$j_XF@Vtj*x#l{%W~iAbN138ZNATPv)?c$ zZia@etI6q0jM~>TLh7jf&RfOz8`RfyRi=KLWV$~RZElV&O4vvrnl>5p`YMVG>Htj}wRT`K%)N<&U zOUvlzKY`d}bp|HYW?7A$hY7wnqAv&(dEhQ0(p~bkia=rPX|Hp8zZqUWnu&JP=x{mMO|oF=_#zl|Dw%D&9`SLS9k9UiGU}z(6Q_@ED1uMrydYLBxCQZzA^NQ9Qm(BxoZXFN3G zT{R_q%k&%N7{T!NZ@Zb;FzZPRy7a%8%^-B$yC%1t)N1z$FCHYug>Ah##dAFI-%Z&BAa^$)=_6(oXMG;o5ma5p7s$A(!vUcm5 z>QwLbDSpw_QA}1Yx;*<~(gDd|VY2$eRmB8UGpYMoN9~pNLAQ^NzO3r7>c4w6&gsQt z_J|?3y|{TV+g3r5V*0|JCURr4DcP#BtN-FC>E)*aEK#&)A|BesVOJG;y#Nj_V z`PfqKPPe_x5T1T~SQpht<5WcDiiI*;8`Q0gTC3j?ff_A?L=oxFRjD=An2naQBDZGG z*VU~?IQ?e}CeMz%wDnxJHaHcQZsLi1>89LrRmAG%jiT}PDVm;|r916Ewo9*^XHpdr zOZnAOALt>3t)qzc3yL4(W`clbHtKnA3eFcWPdwNU!Tu52K_SJxK}xRI`daeIbXth zT2f@*!LT5Xv&%AH^;DBSX;eq@lY1v7{Bw)t_b+NhTEqE!H=VsRD)ng?WNAjP16GNf z_Qo!n6{(FJ@5eVU0t=-iXsqNOzYEUx#KTdktLGrm1VJN&Pzx(viXEj+IL*SQxxi(P8eaIF}Yx%JyGcj|Ef8jLk)~eh71F4O*l-H-fh8^8RQeGOzec-$5GLBys z^7>m%^pN}q6~E`A$u{6EBW@auVr*W%*ZWj$r~3#3S6u)E@WX)1CF8^Y1L{ehQGE_` zPun);7eh%xYdlLsupA>dDn6sXo{sMipgN3~>@oB>ayC}e@XXGP`>VZQ8DUn}d#n;j zq!@XnApdRoU|?lC&nsF7CdH)1UuufFl8U97+(A%|hytT-5q9tIxpp z)W;PGB%z^?MBh0LLX9D)BCYD`b#CJ8`()zgdP4NsM7c>!Y;QhCF7xruRv6j!H{exA z25hiKl{d&tvYBF27|OjT=JVMl4&iUo+?@@u)DB(xR$7J0*YUKNMXm*V_5;D9u7@Wn$u?w70YT)$GJm%9H>#C8ih)56kQ$73G+ zCE7G)LVxrW-6%W-Pbp$Ordvd65mz-arz;v?1 znwvQ1vtP>}8n)U(VKOzU)6x)oIVSbKFD0O-2S_*n?0z?`p*fyuLO^8hoX=?wwQXi6 zonV*fmM~S(ST*lp7Bp`RI=#Rqzgs;1Wnb-~V%S?&6e>M7+Xm?!^kx<&Qt|hMlkV!Q zjF`zS`R*J+o>W?7nOQ>`F4#^lMO}`sHa*^bmcXsE8Pud4`^9RTz;@kZqn&(TC0X^3 zpKnOn|Mj@eo}3nU=qaIkcXzwI&e`Qs{K?f%f zlrfFUsAJz4nJ{c}YAo{?^X@M#B}x0I4@gk_&X*xM`9@O^LtplKt|5~ z?J5s9-w*~Vz(^Mq;yEm&kv-U2->wbT(3=>;86xdW7fg3&t_*d(0j-BW2N-S)XBZT) zYL|EU`YdjKV|J;Dq*ORM@lN=5K6d}2`5`&?8L3HN(TwYq6V}rhPGa0C=W82G$De8q zp)wjxymTrXJ#HoOe;Oyn*r0FH#PLANozKg1bl?vq4i)9qDX%EZaZ)FTxervDXF1In z1=+hc#EibNUtjXZS_Bi4lVcvG{l;F;d@(0)AunE=dj8G%bM>wF2J~lxRi5z!Wz0Ro z6gSH;xhRAOkoO}WZjlqJjD%+6KTg`Kr}WA3 z?&q`kZfmh~arLuXBEdDqnth&Td*n8 zwJ};UGJTmr6m+7Y(-JbDELVq)vHPW6ZSki;ZbAE>#P$`>o;w6WZ`DcEgkGjAKAlRN z*mzTHfmM&XeWmu2Gjs%3Utc^j&GanrQc0mLXc8MHW_#W~rqJe|X!FgzyT40r zB0twAJ}@<2u6gwuL)4Q%8;gzi)CYka+v*D{RhF3ge>B+yw+uC14ziEZq*vghuhd>> zjou>!zUg!cal8J`H*`6qBZK^06TRP27Tdkcr-9z@z=+g^-|FohIRbHcg%% z^eiI7pz|u(h-*ZH?;_^lBX7~PV%=jsG9JY{{+pS#$yZ2JD{DJ)wG4LKn6=lkJC`lq z!v8v&Vt*lIg!g)qd?b)2$#W2_03J;`Le8 z?J3K4Q3QB_+NP{%t_tl4lFv%9+Kr-N@~T9og^l>+YWxfUT>?Lk7r&K zt~M%3`%v=YZ4HV0wI+MMP+{uUR$UTS=09IY>O-iXE4ey^4^*eszjBVzuQo3zd&!QY zp@>?)GZAag;aB!s?M~Q}y}388qu(5Pi;&fL;#dvxY!5}X9#uy9V+WO2y-33KMmKDK z^qhI2=2B&}{uRNVR33Mk9$Qz;gH?e){sTX-R`f}dmCv(Y$_F{gec&?aHRG15_o}K; z*yMH2k8DbT8J$gu7`~#!1RNP5gm7N<%P;r3_kJ`bBzXPtxs#B788L*o zSdofXPkzdE4tmGP=x4$$P8!MI8uC%;R`g8)uSxa|lWL<#iKsI4_Y}fuWsZdkd3fjj z($=-F>L0uco`*5`Fbl^1Ug$@uMcgvZh=Pb1)q}iGIj>~N4KP}T78-IMc+7h@Q;F@x zGjZS2Fn@h|sm!7NbLZ45wp3Lb&Tvt*0mly)7734mXEi^Av)9?9%sYH@? z6e(WPy*!bMe~Y?Z|CNc`*T#ac2URSuT8A&fG|Yi7x%pc#KC*d`r(7O2H^7+!x{_n|Fe_s)}ofD3gj=kPo@GfI)g{fq)SK_mmuN$f_+N8E%4O_gtC~0N%9(5RrGO9t;kFt3or?)y!ZY}MF0M}EbyR=tg?^d!>|yMJ zz+d)bZVZfoH-k}U26yGCE7l>oe;o}?;V|Kn?McC<#o>Hm)$Prf@$Y5#WoyvoNw&4krPc-NMQ8Q*ZLaDWt+mx7oS7N+>u5s#vY1pu z*;-4GE#GS^^|;oQ!(_@hLQi&OlkuL%w^&&0m#<&He&P??`Udk$0+C5&u0q<^E|*-- zf;7)rDZO3a3K*W2SYeA+#M?&m``9I3e%g?!Kp8Py4U%nn+I&hkmGpOZe~)eB=q#zq z;~d&FjVda6Q*YCD;E0L!SMMvWb)#-U{XEq5dCz#0@Y# zzE8c{#W_)qwqhRI4n!b-&W8!irz6o@am;Wz(?NP6B8h(Kve5Pg5C7}DUWpYqIZW#9 z8PmR!iSAq8kyCsiOu99%hjiCf*uQf5<@KGp>c}0wUXjpp0F37SeKA!k0&nkN>`ax} zbf@xY)(>jOIL}_|T~ z=G-Or+cecD1KAZ6z92#@Fwavk&dcjYSAx*y#!FG5lMOT1b||JfnG&^ujJzI<-ECWL zO%`tJrsq1uLg79FbY^7=`b_}E`OU^ML(Ol$tg7i#*^ z$YCgvj`1(mFF#`D7w)V`H$#!}NFDU3Y*?vQA!~?*GRG6>p>u&cl4jN~1rv-Z<#!h6&dS-nA2vV>iEzQ`-v(6q2ZlUO+yfTR$SjT-lKAvj4Dj zhEH??xDJ5q#ZCa8LIClCu1R)4C=bC(qnCEbb7lZ5rUNjJ4WpNU1(Sff3XIyy%NcDh zNzVXilO14n_8bIYVpUZ{5@PJ1a~@(%!O*H4czI%>&_2xclhSHK)2MbqP7e73Uaq^&-7=nby&R*uLbC6A!F)#Mah!j- zufQxm$Ok|UcH|Fm%IsHZN>?Hh$%VtENY>a8{9+J1>JXk=i2ggeV`pFoh@<*EImH3< zunOOej;Px~OuuY~c7B&Dr)BUo%b}d9oT>W`abl3)Pw|gwjEZR-H~CE!Y9#R9=)ZPl zIkQ}5e_qj;@!cxJqO`>PNQUT2(Iwg~)m;JbZgm5JbMez*I1d@(I4lX~c;jKLiB z{T5hs)!&vde)$yBXT;;YI+n$0@$28Pp025RgPl;}vB6ig%a(L!t@)7aYB_8g#3+PqFh)0DMY%<8rnYI#xLEw6>uF$Fw)!n>kp%_qXA?T1XfV{w$x&<6E~#>Svea>4Zt&{uSHy5dMbG zan`e%-Hdo!$*+(|4fArzm@jk`uZwe6q0ui{sF*;pysR=#mF z^ACcLXDqM6hQL-AUsm&Nn{QV31oH{Q(KDjAU4gf--J|~u=jG8Mc09E?o9s$+AN~7A z(1#QzdJdEOHv))7P@ei3zI%{nRXB+)ib;aC7%E!#BCqwn#Ya98I4uqJGR3w~i+o<0 zrQaDm;%)sh0#yw6-b$awlYdLZ^sjMaVaI-EIXmz{L+5�qx~ik1hIyx34MrGbm-3 zloL)vDvZ#ino+`dGLELnW?9+(xD*Cxh;fo*-9E?&Zs1~7$$#qCJ16Ndo9uMSc|b6d zqm}*B!kXmdJ$`%dPzd=%IALLE$oVQ~o4%g;(GbfP%L}%=;u#zA%_Wf4nB-|J}G*d_LjaYjpAH9ve~KhdKRl!9MEQa9-n6A?i(x zhX+Q5>}7Y&&-7;Tl=4k8(K8u^qHGojoHUz~!q@Kc;-_Kk#TRY_5ni zww?P(J*xJHqvNm=Z`9#ocGEOkpvQ_{ysw^3*|XX*jp5iCX-6wAv|dac3Y38G)_i=` z^T7}P8qd4c2b2!hJ8I>3uvP^==lDGDxWA@aEqXc4H@$MWa4gz^TRQ)@_$zKPLG}{z zA@tF>Qq>M%j?v5Nyp&MTx>TpUcJ7wo!K9jzZ`g*4?Zr9Hi*nU01<&h|lv<|~Xqbn} zMf6+6-EZij@d z6DJ0;#|v@?D!@pJZQPz$;f>gh}PF;8mHG?KEkc$VEUus zORn)J%EpkQ3Bk1={14;BsK$Dv<@8k@=zb~rP6no$TeI05DU1!~sXnfEfR`FQC_cx0 z&tb!U`nkC3vUn-qsoK}p^g;TCyq{}koGu!xB=0^}!r7c)co}}EnU(6r&hw0huN681 zt=gTxQg4myHaQswc5@%dN*l$-yJ($ZHgsY$o+hHM#r=Fl;qY$h^KBiKGVAovnxcTq z>DphdWhHTy4wa6+L!ix2NEf~L@@ph6`t40>4*5v)T74Y5-YC`&2#*mo4Jo>~2DWW> zA6s{B`u$ws7MEcd{1ekq=aWPqmE_t{H*9O<7;yjVO7^_vxDKPyhmItz$ya_9Qm=SA zMujgJ(@GdaAN%z^5gK@=`jH|H_xnVZshrZ)K#Nv#!CW$hH;lIXzA8!;k%6CUWIHtu z$+=U>Yl?=(Z3As0o4fT7(1;!_+3BbyKDD{Z^F{Gl1bO2B)7x7{#T7N{x-m$Cbx4!o z5L|-0lc2#G4esvlPH+$I?!n#NJy_%JH15t_TpJOlA5xRZgL13EM%!OOIm4;3)?b_Amcx}1cepW<;-u`9tm9SEIeF z&t|d}cvPqllit#iOJRw&8xSdhC%}U713M4=Jr}?EaQ|YiGh~@<;4$a`@LnOn=Y2G@ zU(OJ{q6?A&+>P~JhL746t2Poi{@~UU2n=4Aq|D%9e2lGjV0sxHuZV=e1Z8%%H|9lt z^g<{Z;Iqf25jMllkj~Y&ylmCb)R18feBY*r7z;z7FD*8b6n}w{Hk3%Rm<@7Q7BA)5 z33!c>iR^RBEbEe2e^6b3Q|IUfRvAvo1K$8`7BW|i&2w*1iLgOwvfsfUMGXx{J(#e% z&uSK>Byl{{^9gTphz5{34XdPa^sv>EyY?Id8`I^)(=XK8oEDCdcyZsf7*?&1t!>O*%yYMB00!E-a9v^> z%A-*(K6;Ak^->2uM|=7=7W;0Um|^c~GnAlJCQP*-`y`>l<>f3eT@R)^4TPkXu?p-O zzGdw)Ed2!o`n@t55C!=+#$mivRk71sxiIQcf~(M*jmJ=Ux1#qG3rNv5-h#)Pg29f%ONpU?R~pv zxnSGzVA1E~#sa67$hvIohcEZX;^h=d$s}mL$7wWE9{;)F>G5TnjeO%Sr0Ua*yd4 zi2NGw;ZAnim8ZD-T64xH&=fB2W&$rM^yY=o#qun(l~Qv4kFL{tI2grOvQhL1Ss8&5 z6z&3-@Nyk3P!<{9sfVif=eKZ$kuy?7eEE3Z1d)9^0??=G&vsYJ*6zp0K2x7-irp4C zBHiNGtH@KU4p{6#IBw-kTJLlED2Kz}SSf`q}W z*g#owq@3fqo6$pdwt5^8Y%T-SpJtKr!1krgNNF%E&(%TCJV4 zYC;$_A%iaza4**zXUcfu7Q)X&)XEMs?C^r@xd3QZ>d?f$EZXc;EQvec(3zn7d71Py zGz+cOKw$lm!nC~dfC$}@gYtYI3nfjg>mtAF`$Vj%<9H4B+(Nqau<5+W|8r(zG6bXe`^oE$j=!&lqdxWz>{qnpcl%D{q+>1y%U*8y3ngED zV)BwH6njU|RQzYvI1l;r5zCb9l;uUlrg!&>vFRT&%MU^jSNGxm9F-gp38_QUUcZTo zMI?ir>|MU6OVH7%t6j z_SH#hXz`gX*M%#GJbHSX@PeZy8oKtu>1zJlxU+i?*C3x}w`HL*JF%RaZhSYNe^}5W ziNRbxFDXA%;-ZC-S`CRZ4UZkz_@e=6fG;WzyncRkwAq6no&X8~JhB7q>i}rBncBkh z#8@91)>aP)xEcpF@tF?kM)R_j&3n~-=due3%t7QUcWv>ldWQ}}@A)8#yD40Iva1Kz zIp>6N@8~!eseuGr7VEinM)~@AD)KfF_29uo@yX;RnaQ2}s)$8^fVb~@>tH)t(R*np?Dy|cONQ1j%oCF@a_+1pk&3+^M&nl&X`RB@tPD}Nloj)tY zSm?+IAA$O!>=wJL1rsYu6VYatUzzj?^puo(IhXA`j9Z|yS=7=DQv<9s10gWC4UbW) zY@o9a{@aB+J=?1RxJLHaG%q7t_If4kncuRn5FILyhJT>lpIp&(PgBkIxBR4MQeqT| z|13C}CTSoo&pvk!I1{lib236I1|{$P-UjfA>(lqvkbZQ}`dysFQUn(3COi+!Iyj`0W zPh(HY9`#@QLf7cIG z4PNVLXx#4=mI8{@iL{d!#Uej?tBDVLPbq77tG=|5IenTBbT`foNwpWEpB4J}T$fToZ2e$2t@@3*Kp$;_38R zTAX5jOIvN!=`&mK5s*H(8vT$Y&vaD+GC^~W?7n~yuZTN!^;)uLjSEK9i~2GkZ+fWQ;j zfL^B>v`L!EP-2R}Wgm6{}%iVA*LCVv9)}C z;>y2(lXvtjr$)|guC>FDPY=EF(sjv$pAY23a;)Q^S;;5aDw>0*X3T*^#U9c_n( z{wr`X2cpa4N=K6kH$4x2(FeW|F7N7^?~YsNp=;kUhfnLxe@+`I0yE-00>(t%uF{y~ z@W%PDwhS@5(+{b(_{%pW&ih5*A4$^NqwBr91QRxrMRj`@TBvah8>IaX#LY_Q_Ur=P z#2|k)Z;#&ylkF4c?XaaKn|0;*WTZv*g%!g@Lb0_RmSQj%!f=T8v$)YxZ==o9>ZO>J zNoo7mj#UcyH6Q$fPm{S-RgQUj*K&NyfOkKJ?VUh(t7<(y2H1KgmZ_`yo@&6}0WPBm z{E|07Mg+oB`T@@nfT>27r4c<{VWkq1w(ky18rkoadArZZ-7_$jXS_~Azq3tCefY?~ zSMxgC{LKD~TgHd8B_$dF^a9=#H&)RDlfcnWXf%rQ1rzYD*0yLc?};0m-@=6u=#_CU z@(%&<3iu@vJ&6K<_dc`Y4?Ko+g?Tro*3S`|4Qbb)bHsN#1HM-=%-aB|**lN|-upZH z4mkLbjIqKbq2LmD8{}F^QZit(GBW*hB~vgTVRT|ck=&xRzlpo?jBDt};ow3d9s@v_ z`=xql)n=uLJZ0^B6C zVk`Nc0l0<0N1?vf`@@pMtq}c~&RYvvn=~DVa23qV4-uG#ukUxj&g6L?w2XsPB0BkWd z{S!C#kJ3911HmpEQH(%ykBS(zD6icaSLPsw@uB08m}oV(CBgjx z?s}=q>VS{RZT}e<`KI~vb0cWl@(plQLWIG2tMSwkrHMe`UA)mx3<2*aJ0$Jj4FhT- z=YUfnA_e~(Oi-GPI2C`386_s`xDw!HmL0f$?h@Nk_zy#&JCN;KvKH%Tb$tcDb;L`3xlDw1%yRe!-h1TNXTP zu9Jubm-7BsH(oZt!~*gjC&oxTQOR}FNk3TjRdjKG14)8t4k`H&=y^28CR?Q48^YG?Wt42f z0N0r3rd@yx01PB_njzCyIOho)0@t}|a6$tOz)Qk-QBv7BbNZ9K0DH*r#a~U;gzhkn zq`$XL>~-%Em1>!TH!wouUZZyMont|Vf6iW*zcl8>F$egaOnYJ4qS>GA^H6Poxt-dR zx5Om>pf*_sqtOYuUjzL*mJ2ox`|_*~DIXl+#`p&o|f_lRmc-?rPy%uIN4CKYs3OR^k= zj5U7!aqYw;4Ym`INRk}C#WBT2Z%`c}F|+!ZpkbG+z0u`P9`)%g7tAaze6B$p;nWAY zX<_;pyRB(qWTw;qgOFk{NLi_^M?oKqo5O!~ZyLEovyy=+=CF^}lmx{KHrGa*Sf7}Y z-*206*#1LB=wN}u&i&ZmZ9ThbHGf!|5o%UST&7*xJP5W^T!Ij46}$Mfb;8Dero@<) zlKnouWht_AV^bHyt#QK^xecxvb>7>DjyCixe#0G2XX zNqfx=?^=i~KsXMWh+&!K2V$fqxPT=AF0FGA>ualykFZ&6b=d>6x=B#1V zn`~+FpT`0RikKXiQV_I~`&WsQC6+o=V#4tLTMM9bYPc06+WyCJeAGm#fg#3M8E!iC z@!Itn1i~Tw?;&i+;mK zZL_bgoh-Q<8gxj`;s*N*QiX&bvBZnfi?4c3q$w2H)6Pp(chHF|DMf;xia#QN^dpad zVm5~3eqrYewyz30kV36N?%(OHO`UVf{e3UXqI!7}{eTY6tqE^;=fW4$Bi6^YRN7oq z`Z{eqD1Brk;%bC>rYuL5_6cZ1${8A4+MhVLdUnS6ft53P-}`fsLu>_8c5m>qi47^S z<$prg%IkWx($W}=phK1^z*?kAif>z=6>PjQqy=w(olOEv-&iY7v_@TMrex3GV3b5y z>g(<3*E5%i^BmDo_*V&!qBOnK_7%DI zkN2dK^y#k7s4DfrIjOD|_*w&XNKh+1x!9YNd;q%Ea<=#{RE9wt z?>rwvvs8=OhV~9Wg1>QFAvKDLt!a=sC>P;3XyOj$+E4W3m=QXO^g85`uqY!^hq7TS zuSm%ja5!+XBwT-B&UbQbru zuU?P1uq>%7TcOuR`)-XG(*47@fs@`SN1#+|m=Ag6BO)2hQxR;HK{Fs~R@|Qm0~*o} zcTq`_$V*dvHA*`EX||dDIs|cRjNXiqn92m;V*ka`xbJU_{+mUGH|wVUg%d8f-Xk4Z zpo*nb$LlMcL0PeR{fdXxdTnIk4_3*o+-EYtc$^~|P}>cskNJ!jJ|5*ub#ydx&K`BM z7hGCC4o)_YkB^?g`VQ=xg5LH`!KbCx{k5Ec&ZmSOhppwORgGuf`sWguJXIWCbK-Z# z?hvEQHR*Zxu_V|_PsI7KYDF~~jKz2E+{sT?z+#sEi5|kL>A1s3DZo=Ge51Ol%uF{G zaN{e5ASzn7!=Q7&{n6B+DtcksAdl@)F7>0Z&}=xB1Buz>T#A zA_VPgD+eL&YxJ4cWk`vpqDyozVv%AHmEvf$<{e?&>6)s&o2mY`!CMYF5KQySC3->m z@6>pq_S*Eo0Nv9{XHr_l;XlX_1bMS{Y!JxdZjuJdV~;C>lpClVKIgpuzBJ~mDIVt% zqvgFtO}U*KO0G`f*i}X?6p7ot7mfFYf=UrWxlO1c$~<}fK4DXrAJ#{)1!8V5#;Q7$ zxw@TX|FcQKfZ-0Nn!$D5{y3Ay=|8yeA^;EjD=_9^~{Wyn+)bzmkd)d)ekT8kX;7 zhp#c}ZdS$I)rCVBrdu5gfEd9UukmizmWX!|u3BD2mlT}+bVe6eqxrS|$BYx_BYk0fktgX$z$N%AiGiey z3m%e1LaJ%g`hUTx*Nq+{z~FHJ;Mg)>e@=p52e3u;Zu>Ol7t1lD#q3_tAq60z32+Dt ze}=XC!Lxo4AT2rv3x>N>Sr#TfrZ_!0mdNf3>}LMpT*xpl6AGu$BaqHYKT*HJwLkbJ zh#^#f!>gI0)2%h_tBNpiYmI-;5{3==+oh%cx4u!uVPJrjh6~^8ui+SPmd6HuaW2TL zalJJCn*WTtdg5ZGAvDq$w0_u;Qna_c(0{v~ai3MKt8sZwp3&8fDV59;6b7|(-wX5o zRl4`39SvVdyms5;rZ8G;=LVY&tg0cNO`C}l8h)v15~jQKi!3Q z+u>WIA&uCLWP z-Pygpl&v?t*W(bhQ;uz^{L>jAN`CfP+#3OVb2y@F*brL=lN(tlI4b`sFL6U5|D>={sb}nKpU@5Q3Tz^niDu#W$re+joRTOUl#A z3WKbMcaG!L>z(aHyhOY$%$UeCN>LzvaNS#bNZYWIe_8zrKpf$HO6-Tt;sCq`R8rmZ zt6We?pch*&{@v{wP?;M8oUZp6lJ`vN$7fs(eyH}XJ}0+dJ@9Am0Ri4QSzPE)f(TBU zTRdW8h#WLVHH)`mBlqLC~FRmKx?NLW#2g*Pi<7c#R|0ATHohYvYGW( z1X#BKLnU(FA407J&M=P$B8&+WB2tAC)m5Nw!gRJ`GvZ)G?V3zG7jm^20XD=9iy8wT zHQ0)=bfNJDxaVPrxY=NZb~`B^I#T#ZQQwNdpNETyzo0qNe(cKoF;i;qJYzT8Q9ek+ zmxY{i$$LVD!tEz=OOOCCB%KS4)RR>}GB7~M%xoV;rt2#VFl4z$uA&x;C)$rCBlt1n)+ZLYfi`Y2)Agh z^~9AH*{~#j{hsJDCa3FMTjADUfL42#v0k?|NnkuwKpf0eXC7mP{({g zU9oQTUUT$1J>+c8F0q5qo-H8WeEm4!799F@-S6h%g7B*%M(m$F-Nx~-D~ z8jk**{x5@tGhsC3GP!oo!MgTL!@S%mkO2e!Gn)V<08#?{EF%4dGZ27aNX8)I$(ukH zfOtN=yg+?V4wMNzIsh_>{iAjPTGZs4NI(Rx1z5ZLf)gxYBHQfB@BUUk+sC8!f=ZCP zkvu>CH0b6fpGwMJM)EKUNepw244>jNA0qOjVf*6-J_b;g)t3IrZwwItPLk(Ypn+zM zRMn6Z{@I2De)I4A`D<-bwtysMAkpT)uKgd<5DGesMAM++-KPN~6=#vOpv^~NF!aA~ z8z@McL#IZL1GS;|=l>He3N@67Jf}wAz=vo-1h~9j>NL%v+l9vhk{sUs+!av4#eQStoY(HcR-r;jCKf`MpClB!REpy%ov>jnU3JOEaO{d>E{%~bAZiY4lTo? z-ZlI|LUx>91<0p_{nc`J5CM>$@Gtg$9wgs24uGd9%(5tcI;dQU0(#1D8aCrLAvqvp zwFv+w0ENvtKWbIfWRvmJuzS6oXmm9@T$WOpviu26{PzW1elARUBR+2cU$JI{3y`4d z$hzI(Spme$_PaD58P(x>Jvb78E5I7Gnc43=W#eaHw~{=X1LQ$5vaF#2Amem^G*~Q` z2?GS`n@xs$wg0}`XHl9$rZ5D4d=qV@Tj$6;o|*g+E4q{+*@z;D0=Z%>ViIGrk;UQG zH)RcnXmq}FR$veu%E7Gd-W!?aH{Uzfwjin;muib7!PrTA^TJvHA3LsW>qCwDZOcR) z1x|MpRe&)OCoElL19n1Z#OGRzTO3gEwzT^`tP`*CmbGW+WapBBp=)iop!)2j;%GPL zkVQn5+F%R$!Ln3}tjoj!Gv3$A61cbgf? z{Y-l=(<9m7ULIM9gf&7A?%)JrLG3P7=L~>5aX#up`Wz< zo*{UY{>&TKv76M*Z;URfu~rh%48)_8-q^FQlt?cM;#vqZc)IQ< zUXe+NBYA09)yPjq@@?W zJS(%+nKq)phAB&*(CGXI5@kfBxPB{?<9f1#-M{}Yc@^kz*yz9HRi%*KSAVsY6FByB z(cd4vSBng)@8-LFTxDe-_VbK|K9Mg<6kU2lqr0bewVtG3<3s_=C_pPB`5WxK1h4^e z&}e7QXoB{X_-ygTH&><`_|_jDWe9svSWhQAcZr)SA~N@Co_M1n|4>W?x`=`3YbUklp;ZwFbAB|b41U!D`p~2Z~2~vJI zwxOv`ocsI^gp7UP25{ZCNV{++YQ@3xBKH08FA7#J-lcp5x7p=>5aTD`Lg$c7vi><= z9`j9AWqVlZ|14*+^M^}r-nicQ^jWU}BQiGPHcmO3(>@HpkHqanFY(DSdh(s;yeg+f z=jX_^u#o>FzM@X;+3#v_=MnKR>?$LUnT+d@Qmu|wI;H4S4r{)7W3YUpsXSX8weQ4= z-AfdSMZy!Jhj{+1Avo-h<=7`-X~A5c*$gE8bHHK>c|G#E?dUyE=jWKrYLTLch3+O0 zs8ZkABx?!3lywR?%T5@G#y@Gz{|<3?g%zY2AoTOe9KxESc<%NP{<2;x>)V6?{nV z+*EPqK-O%JAbF@H>&c2Rgyg9VIgcS!@kpKk$d8m_%J;wLnn_0`{=N&D1~O>cM(iWf zQGv`{E#6zs`Nl&WL+b{Ncdrp+&p7F+kJF>3wR>gcP%7w zB1`>OH+e;DOZ4e^aiqoDUA2Xli%0c=nTEdot2uKJ+4;*-7(;MLFHj46i-xBsnXnvE(@ zb4N)0Eyc(0@|ZP7o?3eVN_F+4u2zK75+0Nxo>_!hYdW!Ju>uLA(n4@|VgRC@w7jv& zFsmv$1%(l*z7u76kf@T#yicj|S2A2pRl3rN?VK*+3R4+>uum5qyG;9(Wux7Bepb11 z&lB1tY??z79jx8vtG#3s{kkwSpEDlPF`CEdrv6c7)P%0pAztSQ^X@?Z89+04ns2

8$+XP=l#U|mr)63y?Tn+Jtb=m4PNJN;TY?N?g<2; zo*`zSMMqnX>GfQXlFS?$(kFr6Q!RHw&-5lAltBi?GYLY_g^~9~9*T-_)`c(0xoL<| z1hQUYr7*o>2oyr16byEnmmX3{&S(9pD#|u@k^!QQWYtwqCz#+Q0Cd=_NP`=1ILOF> zAPco|J=X;vyhBR=GQGapTHNA>*Gch!>HnryBX@BCoz(9%+ z5F)F1EUnIOoQ^#6_9rWyUkhE(kx!GfQe^{e9d0{}PzgnlD{uPdbbO9oJw1KubgZK~Y9 zV0Fi0@i+4(Cf>sBsqQ(fp*}!MJIEM95~T6>7BBLjbK@EyfmW~M*f%YUGZB? zj4FB7#{-$r8Ni_xs}+JItO8+hq(XU0_h=4RF5S2Pk}iSlMW#3fi{kTj=9~w;*`Gwo z{jTM<1YhtnIW&I$)~Jc7%4o zQTDx?@zwrmlnU-#5sf&5$)Dr}A8L{_Q?PH&D}HIMav?1)UZlkjKv|I&9=SpVOZ6jd z$~+f9iBl+pkAZWO_Nv_06)|CbA9olo{5l^Pg=tCfp1Sr=^@? zzfz3AR)PNcS5A2{1ihtSMp9Z?P3)0%m}Ui?J4P5fXp@5|${A14TL}Qzu6C%ma1V4 zyOl422S}qW1E)Rcw)OAi5BSJu08Ly)Pe%~tH2bI8{v0D(?U&hltLxQ`%=T=Y3 zP^yS>E7Ufu(2t~}4$9|E8_6uPuG&qr+NXXk%8#iRmv)Z}3XzKBU69z5tGl+sdQUq- zW>g9H)H>HIAGJN5DU3ygsS-8DQLNkYpsCZl)UzXEm-%;=6bXTj24%hLZErv@WBQBR z`rJdX0%%5TARpVTi%TW84asX1KBZjT{23%`Z zqHF^B#b>ahx0kCP5^KBJb=t!;zCXxzV%Gz9xV_!@pF*QP?8mF*$!f!QZuyeC{{!q~ z91Y%CLRa%!kh40+gnUrytyBLO*x_?ycAU;C1S1EfKvigk&vb!U^mzpWk^2Qb{mZ|a?lzwyrUcQVoeRvzfWplv zB#cxiaqU-@PUvT3Kt1l`$km}*95qhkJ|WZGU!x~*xZ$&KsJ-jWH_XD`;}8%T>0kAf zANZ>|U2iN?2lC2#zU>){h(FH?4ThpZJMIBs{nr6eH=x~g0wflF2fk}Gs0%={0Qmei zKH8R!A|@pJ)wun3B_L!@EBrDA1a@T=tSWO9A!*e}XW2Zq$XV#+=LxD6%MygULWH^R zu~cgZ*zEHItV07AcgI)J!q?I@Ykk``-~LFszJ>x%!v}+lhy|dvqUt~lb*W&w4Fv0c zLrkTB@v#PUBY>cVF>k(E?3T})!scyYFhf6kOkY^NTay2>Z5Gme!kl{b6zt<&Y%)2$ zE)KiUjIawi*9FBqk4=VQmALxv?$ufudb>)H{|EDtb(cN|l1iPd{SIo^Hr%iCEOA`O6`?KWsqhkEwpAgX6^Ee-e=C_+m_NK7%lj7g?S;KL`aA%lEU04i-_d7t}QoU+QUlb zaS>O!mmV*+WdSmk{YJQ2p#=d6W4)UAd8q)!1)*ZKq6q+q30PQF2g+J$V-|34m_{Ca zHZHl{(asTNPXf)?6p8wcge-&!;3Ktll`6azlGPMrk2-PI4m^DfN%}=tpEnqwmZvE) zyDrZh&?6B5wYC7k(`y^0007xfP`~S79=2DNl zkDF7Fvx0)nWZPJR=nX6Zr#4^;TS3QmbfqC#Y_3thWBf9rx(zquzp81RX(FOuyZ{r6 z2z;0OKUCIicSS}Bd3jKBeuhS#eW#Ic6wPXloq;PT@*qoqc+&$iiv1=2v03X9DAO7r z!M+mFRt!uDzx2vbExxcK;2yF1T-E0EJHcnb$1h@?&O#ZSH=)2t5%pW@@B=|I!%~O- zfUe4EAg`}*6zz`)kLp4#_Eg{aK}gMajiJp(eDj-=b|m%tb1I$}k)lLiJ38nURBqh; zviPb}lC8A2*8;f@YNSqS1@X%}CKNaXw?NTi_zFbknOu=k{0CS)#OBut{{TLDE-|B6 z*GE1mP^qY^L>HP_8!`S`uaR%Kou?i2?Xxz*%NeR(D{>*|=*~6c3SYBnr+jg*+S%X# zO9ToO)ZjgQ1e6D#ByXLl#T0TYqzYFWr1>5n#Um-P*{w3oB!siPPi3%g-#AqRy(L=DVnM-V$1Ic5s1x=w~R%PXr)c5z^Zek ztG`%&t-wD2nU=6oIrwHu7b^L8H~F)Qep7a@?6xX3M2w4_u)>%z{jNagj|NnADVdf^gZbrrnQs-~&NZuE&-T){j?I`xQ9C zZaQ{pU+oaafF23EZ~(D;Rxtq!@=qg(jPeBlKAo8l_WmhYU#xcf9AV0m1u42Vda5_CDHKg6Li z-lIEkP?-*r-#^VaKF#DG-G!}>hDEiMC&jdn=@!*4bK$6fpCH=r)F&ZZ2ovEiHc9W3gffMN9IBM>qJ>C zNwS`sas8ED+hYdaCV6;KYmL#JoA~kO)@FIx^=K*WX#3(}Eu|IZ=+$c+8U)n`GV1$J|+6eD+M>?;Uf83{eB7aQK6LGMysY%72Gl$8)qnd)H-?uMxZx>D!5 zdBhjpMpYH#-_@&l!BhrkE(>4umdZ=fgECV>nLLucY9Gm|*ZHn~eyt=|E%!e>{DQVc z(qsg!^shTMa~c2DNjIE5-xIU{SclW&d_ZhAx^#Pk@7nCwv#cl6@Pu(EA8ytfZs&x3 zEH}OSbhQ@YkE)6+r7+*#wx%2gSF?i0z0hJ1TpkoWE2|=@)DAfn$|G}Vsq0fQ)y6+u zot&biLOzIOI!=G0N|XV)nQesiFY~BcS4ufujiZ)?xpgE%S&)zl$eK?epBy%t{%OKxGfgj!UQ;4h}0_3b6CC!rKe^e-EvY5rG+B zkJvJg--i}QbMJQ8dcu4mG);6Ty&b(_AFHTcmy7EToDWs&eeJ?)*IUufyz12{Te#g} zRHt#u$MYrvyl`7yO5<()y(K4`$?+&zMN}1AZEY7OffneVTH2&@SJC40Y>`X&e*P6F zcRFl8(isDqEfKNPTHFMaNk*lc7~53TdIO6}+t_BmJ9Ozfr5x#Ny=q(Fq8vy|w-QDd z9z7tonB6%i9z};~A$FonIA8+pKa4aRJQzk4b$kaL<9M4bg&Uu4_D4Z|Y7vdnb+VD^ zUvh`^$G#KtYbK6#4Fs`6Dk^kxF)>{+SIz?O9@i5Ux?+d%M)vnL=W97;^p!V0IV(L* zQi_UkDS{XJkvu;3q*nR}B5FTruUhMU+Ydi+Qqt`;RTUC~*JNbkszD}|50g0J9B%rA zgn9^?Ku18mWfQlJ&-MqB5{>U&*>1!0$aaD1UGp8v2NJ|)sVHA?gCVY|g0a;PIveYw+x^V; zpZ@loLMtpRDCGH8M~J;614Wq^*s%mm_xDK9ZbFK>yRU10{ysV5c%}3cXXdb7Q{RV^ zUv1d-=}MPZM*MVI?l-G6D&1^3N%tWl$CXO zN20FbN8&4dvFi#s*()oOz6pCCYNTXtALu)WGs!*)p_bwsV@|g-jaVidUG0f_O6bK@ zxg%};5~OmP?2e52$)y3#xwzrr5w*5|O}1CiViHCSI^z3UP-8{X$h*~r)1DDQf_=7# z#)Y{(+%D$!9MflVB*pH|aq-;y4I{EN3BXqo3pZX_-PjfcpZ7dU-AGw;4g zeO@3fL3ox?#mWE$)ip1sXK%{(DZ`hj)KLf_%n;uIf=9;XnA}o0TW9ly!}PaECh(ws zMZOoKUtq_{G+IQsZ-u|wLtuI2C-rVM>(#JFn?NN{mg?ynK1%p-I7IB&7v5?Txz@F+ zKNuB-MizxLFW?=2?jUtDEc&V@Y5Zbp!{Y(Vfb+hH9m(pB>{D0t?7&c$?Khx$j6J`5Zh+s;;|a^3<#%NU8lX?5i%xN~&NCg?Tms7^bq|ENq^|5SY1J%? zrO?05^hrT!cH?Ev3!hd*8H-<}k+BM!9PliCO}>Xj(l3iPm@Vin!XD#c#Di~!TwO~S zoE|E(fCD3~^#v*_=<9?=_Hg%I2LjA~pn=cuAekmlCf-C*MMIO@1Pn29$wT&?-?21| z3E64`{4{E$=%HR`9UNS=Md_zx^Z8GWS2Ojd`vd3%Y^s9Wcbq2+`%IEF3kg4@7_=z- zIEnmW`A^J__q~RNc+FYT2U(`u#zD&g*;OMGn1@tzE?$TiQLbYn^zl^l?1Urb%~X<^i)y#t(7YmF=4@=dkiZ0cWxiyY}f_A zS}ebg057`gM%^#IpXi9$aVDN%33FM(43E5&AUhrpUYYQ6MW`EuC6X@xXI&w zRVkAU2S=4S(Rt$il29@xtk5^}&Ev^?7Mu~?!Saboh-KWZdB-ZDr}!4(vG@Xu2I`z8vB`{zhnmRDs~^WNFKep z-_ZKKcl(gOdnb{6vyf?M=Y9{OIWLC^vcC5JN}&$R+x_-+xKzGETaD3;!p=s&@_NiP z0N!wJQDR5br!PnSmCIL($L}D1cqFy(o>o{Q%Au1-JepZa&soeDmlj!wnhK~rn83L) zab~43Z+f*yftf>`V_NutktWk(x-7RL@DRl3GjjD;xzSI;pNK7OY2b|FGwFw|``*_p z14JGT__`Dfq7D#!_C{WD#~IC=EGumo;$qzv-->lSxz5o#UD+c;?i&%7e=}6Nz=zYn z^W05aXM;*xdVhIXux^`Qk`l%hx;k=PfZ|*#KKF52l=e_WVQoYQ8^RP&pPqL7MHj8^ zkX&k5cu_Ho^OF2|TakxP{e+in)||XoO(CjXx7|!T*BRzv^5wQ!jY%QyFB6NCJ05Il zW^g2rwBZNF#!a{-)1c&H(!u_%JDQD`|C$HKx7mn`>!;rl8^=tqM=w2cYMxvsTct-T zScP)g@o6(}+vr1@zs`K-8Wf%uTX(>aXr!3IM=UgC*Ig+s&O7n_co_m=u?fz1gW1(A zxPqbsuc}#4hUWb|($Y@7AIE-LM%K1?HJ3)`KH`-Z=Iz}4?fI#6zQPMt*GEbTs$ANk zb``$AR`q+Wtz(T@uCRdCks?dhZMjW+CN^8;%gMOAs3Ebr$UyeA+1>1Ts<-513P#dK z0fmO7q=5~d^mG@Plb`KAi(l=pisTn+Y6fM9y@xa$Cl70q*FDuX7QFp=^Xo(DnylM| zjj}5C=5WZpUmdwWl5%1A*3T*;YIDSCZ+mcy>dp|CR(OpJoFMZvRnv`oF}AaWSWP8w zOe9)?x146IcW|(Y2yWpChz2`0anyo;ot%~B-)^SmiyE06MfqI$3ril1EbVCAfZ zUK8mw z9uxhLN$Iib?U?}Uu$fj9YpbFu-&hm*`XaQcDn zm&hQQ-i07mEosP z{oG{kOba*PJ#!ZY6-TKFY{-WaYOiFkvLYJRMj->=%ZPcnh zto3@Qz&Kl{lSfqc*5d=oy%sAsI@ri0;=(qlJ1|1{fHo}hB1>paazF3zcc_*!l~R3F z%`i_>WwEzb?os&ahhi7W)k}vSE+k>yTcC+skpalEOPYW9P>8aX9jiadWR+rhxJ5xb zbsTLP3CV#&RSuW!*Nrjmuv+N#{Fl3O!Wulq-r4G;cNqQVo#G$|2yx&See+M$PcNC5 zOY;snU+%ro@ro<9gBg147IPXDf60Bp6~(Po$5>;=)FI_}He%K^Cwik{L0FX$Wn-l-5If`C_1=N!ogW!U zdr2F70Ae&W(B|GqK1S_{@ZO|y+Qs5r;$$($hzE{)<@(|Nbhms4v{!!aDX5h7XjOT?$1bkp&6oQLX0B5O-+gv` z#)luCm1#+JaWwV*7nZB)`_5$u;5y7MTiVnjx|eixHmY@>A*%5U2-@a9Kga^drJ9dx zA|x;{0-LGw&mJ(c``&a6-SrTq>X~U~ySehb^UBE^YZhxsv@YPR(FjMm!hQ3T9Z(6uI=r$ZJR`yH;=mEJ37!mO|4zTP)*!d}_Xk2o zpcmQm$+xH?h(K4q=Zlyc@K>L&h^q9pf1e191AVgoef0m=P3!6g9}s+VwYmTOzHV$| z-r93_AfSPd1SyyW`8h{3`p?7sO{~1n{^V7LSpNnh4e+@Je@4pZ9 wRR-d;f0xnsZr~7f;L3&r+MqsPk)I#&J?Y3JpgvdeFMvx#5F${*r}^vu0SvkRuK)l5 literal 0 HcmV?d00001 From a2755a4926c41618c2fbd4da41324f46cca0b7f8 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Thu, 16 Jan 2025 10:01:43 +0900 Subject: [PATCH 27/59] fix(mrm_handler): simplify hazard command choice (#9929) Signed-off-by: Ryuta Kambe --- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 4022bdaadebef..b5f4bbaeefc46 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -115,12 +115,7 @@ void MrmHandler::publishHazardCmd() HazardLightsCommand msg; msg.stamp = this->now(); - if (is_emergency_holding_) { - // turn hazard on during emergency holding - msg.command = HazardLightsCommand::ENABLE; - } else if (isEmergency() && param_.turning_hazard_on.emergency) { - // turn hazard on if vehicle is in emergency state and - // turning hazard on if emergency flag is true + if (param_.turning_hazard_on.emergency && isEmergency()) { msg.command = HazardLightsCommand::ENABLE; } else { msg.command = HazardLightsCommand::NO_COMMAND; From 347a2e48109452c70447ce219a207b5fd75591f1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 16 Jan 2025 13:34:45 +0900 Subject: [PATCH 28/59] fix(goal_planner): fix geometric pull over (#9932) Signed-off-by: kosuke55 --- .../src/pull_over_planner/geometric_pull_over.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index bf2ce86b49bab..44dca13ccfd66 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -28,6 +28,7 @@ namespace autoware::behavior_path_planner GeometricPullOver::GeometricPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward) : PullOverPlannerBase{node, parameters}, + parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_{ lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, is_forward_{is_forward}, From e09193784833044b8597c4d3dd7dda9bf169de74 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Thu, 16 Jan 2025 13:51:37 +0900 Subject: [PATCH 29/59] feat(lane_change): ensure path generation doesn't exceed time limit (#9908) * add time limit for lane change candidate path generation Signed-off-by: mohammad alqudah * apply time limit for frenet method as well Signed-off-by: mohammad alqudah * ensure param update value is valid Signed-off-by: mohammad alqudah * fix param update initial value Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * fix param update initial values Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../README.md | 1 + .../config/lane_change.param.yaml | 1 + .../structs/parameters.hpp | 1 + .../src/manager.cpp | 52 +++++++++++++------ .../src/scene.cpp | 18 +++++-- 5 files changed, 52 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index ee371f8592833..20fd564049133 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -1039,6 +1039,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | Name | Unit | Type | Description | Default value | | :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | +| `time_limit` | [ms] | double | Time limit for lane change candidate path generation | 50.0 | | `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index df9491576a4ee..15eb23daecf95 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: lane_change: + time_limit: 50.0 # [ms] backward_lane_length: 200.0 backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index f0adcb1d69b42..60386f535fc64 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -162,6 +162,7 @@ struct Parameters FrenetPlannerParameters frenet{}; // lane change parameters + double time_limit{50.0}; double backward_lane_length{200.0}; double backward_length_buffer_for_end_of_lane{0.0}; double backward_length_buffer_for_blocking_object{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 07b05ab0ea131..f26cf79e8b120 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -190,6 +190,7 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } // lane change parameters + p.time_limit = getOrDeclareParameter(*node, parameter("time_limit")); p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); p.backward_length_buffer_for_end_of_lane = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); @@ -313,6 +314,17 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortime_limit; + updateParam(parameters, ns + "time_limit", time_limit); + if (time_limit >= 10.0) { + p->time_limit = time_limit; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'time_limit' is not updated because the value (%.3f ms) is not valid, " + "keep current value (%.3f ms)", + time_limit, p->time_limit); + } updateParam(parameters, ns + "backward_lane_length", p->backward_lane_length); updateParam( parameters, ns + "backward_length_buffer_for_end_of_lane", @@ -349,25 +361,27 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.lane_changing_decel_factor); updateParam( parameters, ns + "th_prepare_curvature", p->trajectory.th_prepare_curvature); - int longitudinal_acc_sampling_num = 0; + int longitudinal_acc_sampling_num = p->trajectory.lon_acc_sampling_num; updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num; } else { - RCLCPP_WARN_ONCE( - node_->get_logger(), - "Parameter 'lon_acc_sampling_num' is not updated because the value (%d) is not positive", + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'lon_acc_sampling_num' is not updated because the value (%d) is not " + "positive", longitudinal_acc_sampling_num); } - int lateral_acc_sampling_num = 0; + int lateral_acc_sampling_num = p->trajectory.lat_acc_sampling_num; updateParam(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); if (lateral_acc_sampling_num > 0) { p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num; } else { - RCLCPP_WARN_ONCE( - node_->get_logger(), - "Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not positive", + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not " + "positive", lateral_acc_sampling_num); } @@ -409,8 +423,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.lat_acc_map = lat_acc_map; } else { - RCLCPP_WARN_ONCE( - node_->get_logger(), + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, "Mismatched size for lateral acceleration. Expected size: %lu, but velocity: %lu, " "min_values: %lu, max_values: %lu", std::max(2ul, velocity.size()), velocity.size(), min_values.size(), max_values.size()); @@ -515,28 +529,32 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorcancel.enable_on_prepare_phase; updateParam(parameters, ns + "enable_on_prepare_phase", enable_on_prepare_phase); if (!enable_on_prepare_phase) { - RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change cancel function is disabled."); + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Lane Change cancel function is disabled."); p->cancel.enable_on_prepare_phase = enable_on_prepare_phase; } - bool enable_on_lane_changing_phase = true; + bool enable_on_lane_changing_phase = p->cancel.enable_on_lane_changing_phase; updateParam( parameters, ns + "enable_on_lane_changing_phase", enable_on_lane_changing_phase); if (!enable_on_lane_changing_phase) { - RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change abort function is disabled."); + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Lane Change abort function is disabled."); p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase; } - int deceleration_sampling_num = 0; + int deceleration_sampling_num = p->cancel.deceleration_sampling_num; updateParam(parameters, ns + "deceleration_sampling_num", deceleration_sampling_num); if (deceleration_sampling_num > 0) { p->cancel.deceleration_sampling_num = deceleration_sampling_num; } else { - RCLCPP_WARN_ONCE( - node_->get_logger(), + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, "Parameter 'deceleration_sampling_num' is not updated because the value (%d) is not " "positive", deceleration_sampling_num); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 7748795851865..45fcd6fa99086 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1154,18 +1154,22 @@ bool NormalLaneChange::get_path_using_frenet( const std::vector> & sorted_lane_ids, LaneChangePaths & candidate_paths) const { - stop_watch_.tic("frenet_candidates"); + stop_watch_.tic(__func__); constexpr auto found_safe_path = true; const auto frenet_candidates = utils::lane_change::generate_frenet_candidates( common_data_ptr_, prev_module_output_.path, prepare_metrics); RCLCPP_DEBUG( logger_, "Generated %lu candidate paths in %2.2f[us]", frenet_candidates.size(), - stop_watch_.toc("frenet_candidates")); + stop_watch_.toc(__func__)); candidate_paths.reserve(frenet_candidates.size()); lane_change_debug_.frenet_states.clear(); lane_change_debug_.frenet_states.reserve(frenet_candidates.size()); for (const auto & frenet_candidate : frenet_candidates) { + if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) { + break; + } + lane_change_debug_.frenet_states.emplace_back( frenet_candidate.prepare_metric, frenet_candidate.lane_changing.sampling_parameter, frenet_candidate.max_lane_changing_length); @@ -1186,7 +1190,7 @@ bool NormalLaneChange::get_path_using_frenet( if (check_candidate_path_safety(*candidate_path_opt, target_objects)) { RCLCPP_DEBUG( logger_, "Found safe path after %lu candidate(s). Total time: %2.2f[us]", - frenet_candidates.size(), stop_watch_.toc("frenet_candidates")); + frenet_candidates.size(), stop_watch_.toc("__func__")); utils::lane_change::append_target_ref_to_candidate( *candidate_path_opt, common_data_ptr_->lc_param_ptr->frenet.th_curvature_smoothing); candidate_paths.push_back(*candidate_path_opt); @@ -1204,7 +1208,7 @@ bool NormalLaneChange::get_path_using_frenet( RCLCPP_DEBUG( logger_, "No safe path after %lu candidate(s). Total time: %2.2f[us]", frenet_candidates.size(), - stop_watch_.toc("frenet_candidates")); + stop_watch_.toc(__func__)); return !found_safe_path; } @@ -1214,6 +1218,7 @@ bool NormalLaneChange::get_path_using_path_shifter( const std::vector> & sorted_lane_ids, LaneChangePaths & candidate_paths) const { + stop_watch_.tic(__func__); const auto & target_lanes = get_target_lanes(); candidate_paths.reserve( prepare_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); @@ -1279,6 +1284,11 @@ bool NormalLaneChange::get_path_using_path_shifter( prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity); for (const auto & lc_metric : lane_changing_metrics) { + if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) { + RCLCPP_DEBUG(logger_, "Time limit reached and no safe path was found."); + return false; + } + debug_metrics.lc_metrics.emplace_back(lc_metric, -1); const auto debug_print_lat = [&](const std::string & s) { From 099591a740cec19de99c4590f7b468ab9e1d5572 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 16 Jan 2025 14:01:58 +0900 Subject: [PATCH 30/59] chore(simple_planning_simulator): add maintainer (#9934) --- .github/CODEOWNERS | 2 +- simulator/simple_planning_simulator/package.xml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 5bbc4d1c6177b..4fea7c85e1926 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -204,7 +204,7 @@ simulator/autoware_carla_interface/** maxime.clement@tier4.jp mradityagio@gmail. simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp -simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp kotaro.yoshimoto@tier4.jp simulator/tier4_dummy_object_rviz_plugin/** yukihiro.saito@tier4.jp simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp system/autoware_component_monitor/** baris@leodrive.ai memin@leodrive.ai yavuz@leodrive.ai diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index fdc782182f9a5..5bd7a2f74eacc 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -10,6 +10,7 @@ Mamoru Sobue Zulfaqar Azmi Temkei Kem + Kotaro Yoshimoto Apache License 2.0 ament_cmake_auto From 01a764782c86e614a27808b973d073c12da2217e Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Thu, 16 Jan 2025 14:23:18 +0900 Subject: [PATCH 31/59] feat(autoware_shape_estimation): tier4_debug_msgs chnaged to autoware_internal_debug_msgs in autoware_shape_estimation (#9897) feat: tier4_debug_msgs chnaged to autoware_internal_debug_msgs in files perception/autoware_shape_estimation Signed-off-by: vish0012 --- .../autoware_shape_estimation/src/shape_estimation_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp index c5450dc14adb0..28b2a6e398e91 100644 --- a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp +++ b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp @@ -179,9 +179,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared // Publish pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(pub_, output_msg.header.stamp); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace autoware::shape_estimation From 415e1ec0bc0e1a7f446c664f4161a1219f48ca36 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 16 Jan 2025 17:27:22 +0900 Subject: [PATCH 32/59] fix: remove unnecessary parameters (#9935) Signed-off-by: Takayuki Murooka --- .../autoware_pid_longitudinal_controller/README.md | 2 -- .../param/lateral/mpc.param.yaml | 2 -- .../param/longitudinal/pid.param.yaml | 2 -- .../config/crosswalk.param.yaml | 2 -- .../README.md | 13 ++++++------- 5 files changed, 6 insertions(+), 15 deletions(-) diff --git a/control/autoware_pid_longitudinal_controller/README.md b/control/autoware_pid_longitudinal_controller/README.md index aba0f36f04d65..957379866b62a 100644 --- a/control/autoware_pid_longitudinal_controller/README.md +++ b/control/autoware_pid_longitudinal_controller/README.md @@ -195,8 +195,6 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | stopped_state_entry_vel | double | threshold of the ego velocity in transition to the STOPPED state [m/s] | 0.01 | | stopped_state_entry_acc | double | threshold of the ego acceleration in transition to the STOPPED state [m/s^2] | 0.1 | | emergency_state_overshoot_stop_dist | double | If `enable_overshoot_emergency` is true and the ego is `emergency_state_overshoot_stop_dist`-meter ahead of the stop point, the state will transit to EMERGENCY. [m] | 1.5 | -| emergency_state_traj_trans_dev | double | If the ego's position is `emergency_state_traj_tran_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m] | 3.0 | -| emergency_state_traj_rot_dev | double | If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad] | 0.784 | ### DRIVE Parameter diff --git a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml index b358e95f86f99..1d30a28d05cb8 100644 --- a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml @@ -3,8 +3,6 @@ # -- system -- traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) - admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value - admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory # -- path smoothing -- diff --git a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index 5c5d65a13b2d5..f3139308db37c 100644 --- a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -16,8 +16,6 @@ stopped_state_entry_vel: 0.01 stopped_state_entry_acc: 0.1 emergency_state_overshoot_stop_dist: 1.5 - emergency_state_traj_trans_dev: 3.0 - emergency_state_traj_rot_dev: 0.7854 # drive state kp: 1.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index bc0d4f613f10b..97e00ade8256c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -13,8 +13,6 @@ # For the Lanelet2 map with no explicit stop lines stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk - # For the case where the crosswalk width is very wide - far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). # For the case where the stop position is determined according to the object position. stop_distance_from_object_preferred: 3.0 # [m] stop_distance_from_object_limit: 3.0 # [m] diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md index efe4a9a353ecc..963d75e5a8492 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md @@ -12,13 +12,12 @@ This module is activated when there is a stop line in a target lane. ## Module Parameters -| Parameter | Type | Description | -| -------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `stop_margin` | double | Margin that the vehicle tries to stop in before stop_line | -| `stop_duration_sec` | double | [s] Time parameter for the ego vehicle to stop before stop line | -| `hold_stop_margin_distance` | double | [m] Parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | -| `use_initialization_stop_state` | bool | Flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | -| `show_stop_line_collision_check` | bool | Flag to determine whether to show the debug information of collision check with a stop line | +| Parameter | Type | Description | +| ------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_margin` | double | Margin that the vehicle tries to stop in before stop_line | +| `stop_duration_sec` | double | [s] Time parameter for the ego vehicle to stop before stop line | +| `hold_stop_margin_distance` | double | [m] Parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | +| `use_initialization_stop_state` | bool | Flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | ## Inner-workings / Algorithms From cb33b74ccd35b9b29bc11daf83209d9db72c6ab4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 16 Jan 2025 17:27:46 +0900 Subject: [PATCH 33/59] feat(velocity_smoother): introduce diagnostics (#9933) * feat(velocity_smoother): introduce diagnostics Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../include/autoware/velocity_smoother/node.hpp | 11 ++++++----- .../autoware_velocity_smoother/src/node.cpp | 17 +++++++++-------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index fc0066b1a84f3..d519356c7aa18 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -20,6 +20,7 @@ #include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/ros/self_pose_listener.hpp" @@ -45,8 +46,7 @@ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary -#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary +#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary #include "visualization_msgs/msg/marker_array.hpp" #include @@ -61,6 +61,7 @@ namespace autoware::velocity_smoother using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using autoware::universe_utils::DiagnosticsInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_internal_debug_msgs::msg::Float64Stamped; @@ -68,8 +69,7 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary -using tier4_planning_msgs::msg::VelocityLimit; // temporary +using tier4_planning_msgs::msg::VelocityLimit; // temporary using visualization_msgs::msg::MarkerArray; struct Motion @@ -89,7 +89,6 @@ class VelocitySmootherNode : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; - rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; rclcpp::Subscription::SharedPtr sub_current_trajectory_; autoware::universe_utils::InterProcessPollingSubscriber sub_current_odometry_{ this, "/localization/kinematic_state"}; @@ -290,6 +289,8 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr published_time_publisher_; mutable std::shared_ptr time_keeper_{nullptr}; + + std::unique_ptr diagnostics_interface_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 1e0f6119ac21e..1f2b1cad6e42a 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -36,7 +36,8 @@ namespace autoware::velocity_smoother { VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) -: Node("velocity_smoother", node_options) +: Node("velocity_smoother", node_options), + diagnostics_interface_(std::make_unique(this, "velocity_smoother")) { using std::placeholders::_1; @@ -63,7 +64,6 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti pub_velocity_limit_ = create_publisher( "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local()); pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); - pub_over_stop_velocity_ = create_publisher("~/stop_speed_exceeded", 1); sub_current_trajectory_ = create_subscription( "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); @@ -444,6 +444,7 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); + diagnostics_interface_->clear(); base_traj_raw_ptr_ = msg; // receive data @@ -524,6 +525,10 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr // Publish Calculation Time publishStopWatchTime(); + + // Publish diagnostics + diagnostics_interface_->publish(now()); + RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n"); } @@ -906,12 +911,8 @@ void VelocitySmootherNode::overwriteStopPoint( input_stop_vel, output_stop_vel, over_stop_velocity_warn_thr_); } - { - StopSpeedExceeded msg{}; - msg.stamp = this->now(); - msg.stop_speed_exceeded = is_stop_velocity_exceeded; - pub_over_stop_velocity_->publish(msg); - } + diagnostics_interface_->add_key_value( + "The velocity on the stop point is larger than 0.", is_stop_velocity_exceeded); } void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const From ed1f844ce77f496cc44894d23492646354a12217 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Thu, 16 Jan 2025 17:36:00 +0900 Subject: [PATCH 34/59] test(start_planner): disable GenerateValidFreespacePullOutPath test (#9937) Signed-off-by: kyoichi-sugahara --- .../test/test_freespace_pull_out.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp index 79c58c37f0dcf..5078c463d7c36 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp @@ -78,7 +78,7 @@ class TestFreespacePullOut : public ::testing::Test } }; -TEST_F(TestFreespacePullOut, GenerateValidFreespacePullOutPath) +TEST_F(TestFreespacePullOut, DISABLED_GenerateValidFreespacePullOutPath) { const auto start_pose = geometry_msgs::build() From cde1c78dd8d971514650793448f177a688667321 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 16 Jan 2025 18:30:31 +0900 Subject: [PATCH 35/59] feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (#9927) Signed-off-by: satoshi-ota Signed-off-by: Mamoru Sobue Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: satoshi-ota --- .../planner_interface.hpp | 11 +- .../obstacle_cruise_planner/type_alias.hpp | 5 - .../obstacle_cruise_planner/utils.hpp | 3 - .../package.xml | 1 - .../src/node.cpp | 1 + .../pid_based_planner/pid_based_planner.cpp | 7 +- .../src/planner_interface.cpp | 18 +- .../src/utils.cpp | 21 -- .../package.xml | 1 - .../src/debug_marker.cpp | 33 +-- .../src/debug_marker.hpp | 14 +- .../src/interface.cpp | 4 +- .../src/interface.hpp | 3 +- .../src/manager.cpp | 2 +- .../manager.hpp | 2 +- .../scene.hpp | 3 +- .../src/scene.cpp | 5 +- .../src/manager.cpp | 4 +- .../goal_planner_module.hpp | 5 +- .../src/goal_planner_module.cpp | 26 +-- .../src/manager.cpp | 2 +- .../interface.hpp | 1 + .../src/interface.cpp | 48 ++-- .../src/manager.cpp | 2 +- .../behavior_path_planner_node.hpp | 7 +- .../src/behavior_path_planner_node.cpp | 39 ++-- .../src/planner_manager.cpp | 3 +- .../interface/scene_module_interface.hpp | 44 ++-- .../scene_module_manager_interface.hpp | 50 +--- .../scene_module_manager_interface.cpp | 9 +- .../manager.hpp | 2 +- .../sampling_planner_module.hpp | 3 +- .../src/sampling_planner_module.cpp | 5 +- .../manager.hpp | 2 +- .../behavior_path_side_shift_module/scene.hpp | 3 +- .../src/scene.cpp | 5 +- .../manager.hpp | 2 +- .../start_planner_module.hpp | 11 +- .../src/start_planner_module.cpp | 39 ++-- .../manager.hpp | 2 +- .../scene.hpp | 15 +- .../src/scene.cpp | 20 +- .../scene.hpp | 4 +- .../src/decisions.cpp | 14 +- .../src/manager.cpp | 2 +- .../src/scene.cpp | 7 +- .../src/manager.cpp | 4 +- .../src/scene_crosswalk.cpp | 23 +- .../src/scene_crosswalk.hpp | 4 +- .../src/manager.cpp | 3 +- .../src/scene.cpp | 6 +- .../src/scene.hpp | 4 +- .../src/manager.cpp | 9 +- .../src/scene_intersection.cpp | 167 ++++++++----- .../src/scene_intersection.hpp | 4 +- .../src/scene_merge_from_private_road.cpp | 13 +- .../src/scene_merge_from_private_road.hpp | 4 +- .../src/manager.cpp | 3 +- .../src/scene.cpp | 8 +- .../src/scene.hpp | 4 +- .../src/manager.cpp | 2 +- .../src/scene_no_stopping_area.cpp | 7 +- .../src/scene_no_stopping_area.hpp | 4 +- .../src/manager.cpp | 4 +- .../src/scene_occlusion_spot.cpp | 7 +- .../src/scene_occlusion_spot.hpp | 4 +- .../scene_module_interface.hpp | 22 +- .../src/scene_module_interface.cpp | 7 +- .../scene_module_interface_with_rtc.hpp | 4 +- .../src/scene_module_interface_with_rtc.cpp | 6 +- .../src/manager.cpp | 3 +- .../src/scene.cpp | 22 +- .../src/scene.hpp | 4 +- .../src/manager.cpp | 2 +- .../src/scene.cpp | 7 +- .../src/scene.hpp | 5 +- .../src/manager.cpp | 2 +- .../src/scene.cpp | 7 +- .../src/scene.hpp | 5 +- .../test/test_scene.cpp | 5 +- .../src/manager.cpp | 5 +- .../src/scene.cpp | 7 +- .../src/scene.hpp | 6 +- .../src/manager.cpp | 3 +- .../src/scene.cpp | 6 +- .../src/scene.hpp | 4 +- .../src/manager.cpp | 3 +- .../src/scene.cpp | 7 +- .../src/scene.hpp | 5 +- .../src/manager.cpp | 3 +- .../src/scene_walkway.cpp | 16 +- .../src/scene_walkway.hpp | 4 +- .../src/dynamic_obstacle_stop_module.cpp | 16 +- .../src/dynamic_obstacle_stop_module.hpp | 1 + .../src/obstacle_velocity_limiter_module.cpp | 1 - .../src/out_of_lane_module.cpp | 16 +- .../src/out_of_lane_module.hpp | 1 + .../plugin_module_interface.hpp | 10 +- .../velocity_planning_result.hpp | 2 - .../README.md | 8 +- .../src/node.cpp | 10 - .../src/node.hpp | 2 - .../src/planner_manager.cpp | 2 + system/autoware_default_adapi/package.xml | 1 + .../autoware_default_adapi/src/planning.cpp | 221 ++++++++++++++---- .../autoware_default_adapi/src/planning.hpp | 20 +- 106 files changed, 731 insertions(+), 544 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index ef6ae1662dcee..823771e173c48 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ +#include "autoware/motion_utils/factor/planning_factor_interface.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/obstacle_cruise_planner/common_structs.hpp" #include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp" @@ -47,15 +48,15 @@ class PlannerInterface rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) - : longitudinal_info_(longitudinal_info), + : planning_factor_interface_{std::make_unique( + &node, "obstacle_cruise_planner")}, + longitudinal_info_(longitudinal_info), vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param), debug_data_ptr_(debug_data_ptr), slow_down_param_(SlowDownParam(node)), stop_param_(StopParam(node, longitudinal_info)) { - velocity_factors_pub_ = - node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); metrics_pub_ = node.create_publisher("~/metrics", 10); @@ -101,6 +102,7 @@ class PlannerInterface const std::optional & stop_pose = std::nullopt, const std::optional & stop_obstacle = std::nullopt); void publishMetrics(const rclcpp::Time & current_time); + void publishPlanningFactors() { planning_factor_interface_->publish(); } void clearMetrics(); void onParam(const std::vector & parameters) @@ -128,6 +130,8 @@ class PlannerInterface double getSafeDistanceMargin() const { return longitudinal_info_.safe_distance_margin; } protected: + std::unique_ptr planning_factor_interface_; + // Parameters bool enable_debug_info_{false}; bool enable_calculation_time_info_{false}; @@ -145,7 +149,6 @@ class PlannerInterface stop_watch_; // Publishers - rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index 04badd2a956ef..31344903b6809 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -17,8 +17,6 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp" -#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" #include "autoware_perception_msgs/msg/predicted_object.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -39,9 +37,6 @@ #include using autoware::vehicle_info_utils::VehicleInfo; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp index d929647d9bcd0..ebabd96a54608 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp @@ -94,9 +94,6 @@ size_t getIndexWithLongitudinalOffset( return 0; } -VelocityFactorArray makeVelocityFactorArray( - const rclcpp::Time & time, const std::string & behavior = PlanningBehavior::ROUTE_OBSTACLE, - const std::optional pose = std::nullopt); } // namespace obstacle_cruise_utils #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 8583d0639e5ce..7c6d0b286c9b0 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -18,7 +18,6 @@ ament_cmake_auto autoware_cmake - autoware_adapi_v1_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 6931ff58e8457..4242f0d559946 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -726,6 +726,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 8. Publish debug data published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); planner_ptr_->publishMetrics(now()); + planner_ptr_->publishPlanningFactors(); publishDebugMarker(); publishDebugInfo(); objects_of_interest_marker_interface_.publishMarkerArray(); diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 544597f05ff75..20b564addf149 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -335,9 +335,10 @@ std::vector PIDBasedPlanner::planCruise( debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data); - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ADAPTIVE_CRUISE, - stop_traj_points.at(wall_idx).pose)); + planning_factor_interface_->add( + stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::NONE, + tier4_planning_msgs::msg::SafetyFactorArray{}); } // do cruise planning diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index b08bb10ef69cf..b32d7f3a1bcbb 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -182,8 +182,6 @@ std::vector PlannerInterface::generateStopTrajectory( : std::abs(vehicle_info_.min_longitudinal_offset_m); if (stop_obstacles.empty()) { - velocity_factors_pub_->publish( - obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time)); // delete marker const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); @@ -336,8 +334,10 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose)); + planning_factor_interface_->add( + output_traj_points, planner_data.ego_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); // Store stop reason debug data debug_data_ptr_->stop_metrics = makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle); @@ -590,10 +590,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, - slow_down_traj_points.at(slow_down_wall_idx).pose)); autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + planning_factor_interface_->add( + slow_down_traj_points, planner_data.ego_pose, + slow_down_traj_points.at(*slow_down_start_idx).pose, + slow_down_traj_points.at(*slow_down_end_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + tier4_planning_msgs::msg::SafetyFactorArray{}, planner_data.is_driving_forward, + stable_slow_down_vel); } // add debug virtual wall diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index 82f4e6978181f..b27def0bfcbe5 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -118,25 +118,4 @@ std::vector getClosestStopObstacles(const std::vector pose) -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = time; - - if (pose) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = behavior; - velocity_factor.pose = pose.value(); - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - } // namespace obstacle_cruise_utils diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 4a6f946b8eb90..8bbc0b6bf4e02 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -14,7 +14,6 @@ autoware_cmake eigen3_cmake_module - autoware_adapi_v1_msgs autoware_motion_utils autoware_perception_msgs autoware_planning_msgs diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index bbd1cac04ed89..2acf6ba1c92f5 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -67,7 +67,9 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const double & surround_check_back_distance, const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) -: vehicle_info_(vehicle_info), +: planning_factor_interface_{std::make_unique( + &node, "surround_obstacle_checker")}, + vehicle_info_(vehicle_info), object_label_(object_label), surround_check_front_distance_(surround_check_front_distance), surround_check_side_distance_(surround_check_side_distance), @@ -77,8 +79,6 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( clock_(clock) { debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); - velocity_factor_pub_ = - node.create_publisher("/planning/velocity_factors/surround_obstacle", 1); vehicle_footprint_pub_ = node.create_publisher("~/debug/footprint", 1); vehicle_footprint_offset_pub_ = node.create_publisher("~/debug/footprint_offset", 1); @@ -143,8 +143,12 @@ void SurroundObstacleCheckerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* publish stop reason for autoware api */ - const auto velocity_factor_msg = makeVelocityFactorArray(); - velocity_factor_pub_->publish(velocity_factor_msg); + if (stop_pose_ptr_ != nullptr) { + planning_factor_interface_->add( + 0.0, *stop_pose_ptr_, tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); + } + planning_factor_interface_->publish(); /* reset variables */ stop_pose_ptr_ = nullptr; @@ -170,25 +174,6 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() return msg; } -VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - - if (stop_pose_ptr_) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = PlanningBehavior::SURROUNDING_OBSTACLE; - velocity_factor.pose = *stop_pose_ptr_; - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( const Polygon2d & boost_polygon, const double & z) { diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index b2c350c1b4698..c49e277f2dc6c 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -15,13 +15,13 @@ #ifndef DEBUG_MARKER_HPP_ #define DEBUG_MARKER_HPP_ +#include #include #include -#include -#include #include #include +#include #include #include @@ -34,10 +34,10 @@ namespace autoware::surround_obstacle_checker { using autoware::vehicle_info_utils::VehicleInfo; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using geometry_msgs::msg::PolygonStamped; +using tier4_planning_msgs::msg::ControlPoint; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -65,12 +65,13 @@ class SurroundObstacleCheckerDebugNode private: rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; + std::unique_ptr planning_factor_interface_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::string object_label_; double surround_check_front_distance_; @@ -80,7 +81,6 @@ class SurroundObstacleCheckerDebugNode geometry_msgs::msg::Pose self_pose_; MarkerArray makeVisualizationMarker(); - VelocityFactorArray makeVelocityFactorArray(); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 3080ba1045b41..37bee1a178f90 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -32,13 +32,15 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface) : LaneChangeInterface{ name, node, parameters, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, + planning_factor_interface, std::make_unique(parameters, avoidance_by_lane_change_parameters)} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp index c7fbba34b1adb..6e524f3c91341 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp @@ -40,7 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface); bool isExecutionRequested() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index c54774a575332..2bf8a60676d72 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -190,7 +190,7 @@ SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index f09196b2cc8e1..ea8f09c4f9336 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -42,7 +42,7 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 44b56f4ea0990..8987e7c0446ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -351,7 +351,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); void updateModuleParams(const std::any & parameters) override { diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index dc7efebe88fba..145e0df2675b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -331,8 +331,9 @@ DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule( std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{std::move(parameters)}, target_objects_manager_{TargetObjectsManager( parameters_->successive_num_to_entry_dynamic_avoidance_condition, diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index 45c997f364633..0b438d1a1758e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -28,7 +28,7 @@ ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, direction_)); } @@ -37,7 +37,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 41524b1b8193c..84367fdd91b80 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -240,7 +240,8 @@ class GoalPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); ~GoalPlannerModule() { @@ -455,7 +456,7 @@ class GoalPlannerModule : public SceneModuleInterface // steering factor void updateSteeringFactor( const PullOverContextData & context_data, const std::array & pose, - const std::array distance, const uint16_t type); + const std::array distance); // rtc std::pair calcDistanceToPathChange( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 0a6185127fcf0..c7e32a47a59f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -66,8 +66,9 @@ GoalPlannerModule::GoalPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_lane_parking_cb_running_{false}, @@ -120,9 +121,6 @@ GoalPlannerModule::GoalPlannerModule( initializeSafetyCheckParameters(); } - steering_factor_interface_.init(PlanningBehavior::GOAL_PLANNER); - velocity_factor_interface_.init(PlanningBehavior::GOAL_PLANNER); - /** * NOTE: Add `universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);` to functions called * from the main thread only. @@ -1344,19 +1342,20 @@ void GoalPlannerModule::setTurnSignalInfo( void GoalPlannerModule::updateSteeringFactor( const PullOverContextData & context_data, const std::array & pose, - const std::array distance, const uint16_t type) + const std::array distance) { - const uint16_t steering_factor_direction = std::invoke([&]() { + const uint16_t planning_factor_direction = std::invoke([&]() { const auto turn_signal = calcTurnSignalInfo(context_data); if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; } - return SteeringFactor::STRAIGHT; + return PlanningFactor::NONE; }); - steering_factor_interface_.set(pose, distance, steering_factor_direction, type, ""); + planning_factor_interface_->add( + distance[0], distance[1], pose[0], pose[1], planning_factor_direction, SafetyFactorArray{}); } void GoalPlannerModule::decideVelocity(PullOverPath & pull_over_path) @@ -1568,10 +1567,9 @@ void GoalPlannerModule::postProcess() updateSteeringFactor( context_data, {pull_over_path.start_pose(), pull_over_path.modified_goal_pose()}, - {distance_to_path_change.first, distance_to_path_change.second}, - has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); + {distance_to_path_change.first, distance_to_path_change.second}); - setVelocityFactor(pull_over_path.full_path()); + set_longitudinal_planning_factor(pull_over_path.full_path()); } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 6bbc34a495080..a99252c923328 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -31,7 +31,7 @@ std::unique_ptr GoalPlannerModuleManager::createNewSceneMo { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 4b97fb0d069a0..82b042d1135d2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -53,6 +53,7 @@ class LaneChangeInterface : public SceneModuleInterface const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface, std::unique_ptr && module_type); LaneChangeInterface(const LaneChangeInterface &) = delete; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index ebcd4eb4809fc..4519b20d4644d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -40,15 +40,14 @@ LaneChangeInterface::LaneChangeInterface( const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface, std::unique_ptr && module_type) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{std::move(parameters)}, module_type_{std::move(module_type)} { module_type_->setTimeKeeper(getTimeKeeper()); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); - steering_factor_interface_.init(PlanningBehavior::LANE_CHANGE); - velocity_factor_interface_.init(PlanningBehavior::LANE_CHANGE); } void LaneChangeInterface::processOnExit() @@ -145,7 +144,7 @@ BehaviorModuleOutput LaneChangeInterface::plan() } } - setVelocityFactor(output.path); + set_longitudinal_planning_factor(output.path); return output; } @@ -179,7 +178,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() isExecutionReady(), State::WAITING_FOR_EXECUTION); is_abort_path_approved_ = false; - setVelocityFactor(out.path); + set_longitudinal_planning_factor(out.path); return out; } @@ -384,15 +383,6 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); - const auto steering_factor_direction = std::invoke([&]() { - if (module_type_->getDirection() == Direction::LEFT) { - return SteeringFactor::LEFT; - } - if (module_type_->getDirection() == Direction::RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::UNKNOWN; - }); const auto current_position = module_type_->getEgoPosition(); const auto status = module_type_->getLaneChangeStatus(); @@ -401,24 +391,34 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto finish_distance = autoware::motion_utils::calcSignedArcLength( output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); - steering_factor_interface_.set( - {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, ""); + const auto planning_factor_direction = std::invoke([&]() { + if (module_type_->getDirection() == Direction::LEFT) { + return PlanningFactor::SHIFT_LEFT; + } + if (module_type_->getDirection() == Direction::RIGHT) { + return PlanningFactor::SHIFT_RIGHT; + } + return PlanningFactor::UNKNOWN; + }); + + planning_factor_interface_->add( + start_distance, finish_distance, status.lane_change_path.info.shift_line.start, + status.lane_change_path.info.shift_line.end, planning_factor_direction, SafetyFactorArray{}); } void LaneChangeInterface::updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const { - const uint16_t steering_factor_direction = std::invoke([&output]() { + const uint16_t planning_factor_direction = std::invoke([&output]() { if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; } - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; }); - steering_factor_interface_.set( - {selected_path.info.shift_line.start, selected_path.info.shift_line.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + output.start_distance_to_path_change, output.finish_distance_to_path_change, + selected_path.info.shift_line.start, selected_path.info.shift_line.end, + planning_factor_direction, SafetyFactorArray{}); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index f26cf79e8b120..450497fb2ca29 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -302,7 +302,7 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index ebe7c47e6fab2..7f7ecd7326b08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -20,7 +20,7 @@ #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "planner_manager.hpp" -#include +#include #include #include @@ -51,7 +51,7 @@ namespace autoware::behavior_path_planner { -using autoware::motion_utils::SteeringFactorInterface; +using autoware::motion_utils::PlanningFactorInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObject; @@ -121,7 +121,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr reroute_availability_publisher_; - rclcpp::Publisher::SharedPtr pub_steering_factors_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -136,7 +135,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_manager_; - SteeringFactorInterface steering_factor_interface_; + std::unique_ptr planning_factor_interface_; std::mutex mutex_pd_; // mutex for planner_data_ std::mutex mutex_manager_; // mutex for bt_manager_ or planner_manager_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 7758ab530e88c..568d432e1faa3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -32,7 +32,9 @@ using autoware::vehicle_info_utils::VehicleInfoUtils; using tier4_planning_msgs::msg::PathChangeModuleId; BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options) -: Node("behavior_path_planner", node_options) +: Node("behavior_path_planner", node_options), + planning_factor_interface_{ + std::make_unique(this, "behavior_path_planner")} { using std::placeholders::_1; using std::chrono_literals::operator""ms; @@ -51,8 +53,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const auto durable_qos = rclcpp::QoS(1).transient_local(); modified_goal_publisher_ = create_publisher("~/output/modified_goal", durable_qos); - pub_steering_factors_ = - create_publisher("/planning/steering_factor/intersection", 1); reroute_availability_publisher_ = create_publisher("~/output/is_reroute_available", 1); debug_avoidance_msg_array_publisher_ = @@ -115,8 +115,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod turn_signal_search_time, turn_signal_intersection_angle_threshold_deg); } - steering_factor_interface_.init(PlanningBehavior::INTERSECTION); - // Start timer { const auto planning_hz = declare_parameter("planning_hz"); @@ -463,33 +461,22 @@ void BehaviorPathPlannerNode::publish_steering_factor( const auto [intersection_flag, approaching_intersection_flag] = planner_data->turn_signal_decider.getIntersectionTurnSignalFlag(); if (intersection_flag || approaching_intersection_flag) { - const uint16_t steering_factor_direction = std::invoke([&turn_signal]() { - if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - const auto [intersection_pose, intersection_distance] = planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); - steering_factor_interface_.set( - {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance}, - steering_factor_direction, SteeringFactor::TURNING, ""); - } else { - steering_factor_interface_.reset(); - } - - autoware_adapi_v1_msgs::msg::SteeringFactorArray steering_factor_array; - steering_factor_array.header.frame_id = "map"; - steering_factor_array.header.stamp = this->now(); + const uint16_t planning_factor_direction = std::invoke([&turn_signal]() { + if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return PlanningFactor::TURN_LEFT; + } + return PlanningFactor::TURN_RIGHT; + }); - const auto steering_factor = steering_factor_interface_.get(); - if (steering_factor.behavior != PlanningBehavior::UNKNOWN) { - steering_factor_array.factors.emplace_back(steering_factor); + planning_factor_interface_->add( + intersection_distance, intersection_distance, intersection_pose, intersection_pose, + planning_factor_direction, SafetyFactorArray{}); } - pub_steering_factors_->publish(steering_factor_array); + planning_factor_interface_->publish(); } void BehaviorPathPlannerNode::publish_reroute_availability() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 041b7ad20a107..322209d46732b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -182,8 +182,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); m->publishRTCStatus(); - m->publishSteeringFactor(); - m->publishVelocityFactor(); + m->publish_planning_factors(); }); generateCombinedDrivableArea(result_output.valid_output, data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 391ac92411220..c96bc601b6403 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -21,8 +21,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include -#include -#include +#include #include #include #include @@ -36,7 +35,6 @@ #include #include -#include #include #include #include @@ -54,18 +52,16 @@ namespace autoware::behavior_path_planner { -using autoware::motion_utils::SteeringFactorInterface; -using autoware::motion_utils::VelocityFactorInterface; +using autoware::motion_utils::PlanningFactorInterface; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::generateUUID; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::SteeringFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactor; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; @@ -86,13 +82,15 @@ class SceneModuleInterface const std::string & name, rclcpp::Node & node, std::unordered_map> rtc_interface_ptr_map, std::unordered_map> - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), objects_of_interest_marker_interface_ptr_map_( std::move(objects_of_interest_marker_interface_ptr_map)), + planning_factor_interface_{planning_factor_interface}, time_keeper_(std::make_shared()) { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { @@ -184,8 +182,6 @@ class SceneModuleInterface unlockNewModuleLaunch(); unlockOutputPath(); - reset_factor(); - processOnExit(); } @@ -258,16 +254,6 @@ class SceneModuleInterface ModuleStatus getCurrentStatus() const { return current_state_; } - void reset_factor() - { - steering_factor_interface_.reset(); - velocity_factor_interface_.reset(); - } - - auto get_steering_factor() const -> SteeringFactor { return steering_factor_interface_.get(); } - - auto get_velocity_factor() const -> VelocityFactor { return velocity_factor_interface_.get(); } - std::string name() const { return name_; } PoseWithDetailOpt getStopPose() const @@ -558,11 +544,12 @@ class SceneModuleInterface } } - void setVelocityFactor(const PathWithLaneId & path) + void set_longitudinal_planning_factor(const PathWithLaneId & path) { if (stop_pose_.has_value()) { - velocity_factor_interface_.set( - path.points, getEgoPose(), stop_pose_.value().pose, VelocityFactor::APPROACHING, "stop"); + planning_factor_interface_->add( + path.points, getEgoPose(), stop_pose_.value().pose, PlanningFactor::STOP, + SafetyFactorArray{}); return; } @@ -570,8 +557,9 @@ class SceneModuleInterface return; } - velocity_factor_interface_.set( - path.points, getEgoPose(), slow_pose_.value().pose, VelocityFactor::APPROACHING, "slow down"); + planning_factor_interface_->add( + path.points, getEgoPose(), slow_pose_.value().pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}); } void setDrivableLanes(const std::vector & drivable_lanes); @@ -627,9 +615,7 @@ class SceneModuleInterface std::unordered_map> objects_of_interest_marker_interface_ptr_map_; - mutable SteeringFactorInterface steering_factor_interface_; - - mutable VelocityFactorInterface velocity_factor_interface_; + mutable std::shared_ptr planning_factor_interface_; mutable PoseWithDetailOpt stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 73b133952935b..446829d1d0f9d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -24,8 +24,6 @@ #include #include -#include -#include #include #include @@ -42,8 +40,6 @@ using autoware::motion_utils::createDeadLineVirtualWallMarker; using autoware::motion_utils::createSlowDownVirtualWallMarker; using autoware::motion_utils::createStopVirtualWallMarker; using autoware::universe_utils::toHexString; -using autoware_adapi_v1_msgs::msg::SteeringFactorArray; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; using SceneModuleObserver = std::weak_ptr; @@ -106,45 +102,7 @@ class SceneModuleManagerInterface } } - void publishSteeringFactor() - { - SteeringFactorArray steering_factor_array; - steering_factor_array.header.frame_id = "map"; - steering_factor_array.header.stamp = node_->now(); - - for (const auto & m : observers_) { - if (m.expired()) { - continue; - } - - const auto steering_factor = m.lock()->get_steering_factor(); - if (steering_factor.behavior != PlanningBehavior::UNKNOWN) { - steering_factor_array.factors.emplace_back(steering_factor); - } - } - - pub_steering_factors_->publish(steering_factor_array); - } - - void publishVelocityFactor() - { - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = node_->now(); - - for (const auto & m : observers_) { - if (m.expired()) { - continue; - } - - const auto velocity_factor = m.lock()->get_velocity_factor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - } - - pub_velocity_factors_->publish(velocity_factor_array); - } + void publish_planning_factors() { planning_factor_interface_->publish(); } void publishVirtualWall() const { @@ -304,10 +262,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_drivable_lanes_; - rclcpp::Publisher::SharedPtr pub_steering_factors_; - - rclcpp::Publisher::SharedPtr pub_velocity_factors_; - rclcpp::Publisher::SharedPtr pub_processing_time_; std::string name_; @@ -318,6 +272,8 @@ class SceneModuleManagerInterface std::unique_ptr idle_module_ptr_; + std::shared_ptr planning_factor_interface_; + std::unordered_map> rtc_interface_ptr_map_; std::unordered_map> diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp index b91db3a740cfe..11be3b28a7f94 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp @@ -61,10 +61,11 @@ void SceneModuleManagerInterface::initInterface( pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name_, 20); pub_processing_time_ = node->create_publisher( "~/processing_time/" + name_, 20); - pub_steering_factors_ = - node->create_publisher("/planning/steering_factor/" + name_, 1); - pub_velocity_factors_ = - node->create_publisher("/planning/velocity_factors/" + name_, 1); + } + + // planning factor + { + planning_factor_interface_ = std::make_shared(node, name_); } // misc diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp index 28c310ae20ec2..f25c073e879da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp @@ -38,7 +38,7 @@ class SamplingPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams( diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index c9b75f7fa96de..f84fe45d08ca4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -86,7 +86,8 @@ class SamplingPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index fac3cc92de1d6..5bb57f17ce813 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -42,8 +42,9 @@ SamplingPlannerModule::SamplingPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()} { internal_params_ = std::make_shared(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp index b495e6619a1c7..2b4a20ddc8cca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp @@ -39,7 +39,7 @@ class SideShiftModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp index 15d81ff45abcf..c8a5e47287477 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp @@ -45,7 +45,8 @@ class SideShiftModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index 22ee7ab77e077..9536c1ba17336 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -42,8 +42,9 @@ SideShiftModule::SideShiftModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp index 5d3d224673124..68b1e67e484c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp @@ -38,7 +38,7 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index cf9227c2387f8..9b1186cb5fc66 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -86,7 +86,8 @@ class StartPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); ~StartPlannerModule() override { @@ -198,18 +199,18 @@ class StartPlannerModule : public SceneModuleInterface bool requiresDynamicObjectsCollisionDetection() const; - uint16_t getSteeringFactorDirection( + uint16_t getPlanningFactorDirection( const autoware::behavior_path_planner::BehaviorModuleOutput & output) const { switch (output.turn_signal_info.turn_signal.command) { case TurnIndicatorsCommand::ENABLE_LEFT: - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; case TurnIndicatorsCommand::ENABLE_RIGHT: - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; default: - return SteeringFactor::STRAIGHT; + return PlanningFactor::NONE; } }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 0bac50a37fdbf..ae05bcf4c084e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -62,8 +62,9 @@ StartPlannerModule::StartPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} @@ -89,9 +90,6 @@ StartPlannerModule::StartPlannerModule( std::bind(&StartPlannerModule::onFreespacePlannerTimer, this), freespace_planner_timer_cb_group_); } - - steering_factor_interface_.init(PlanningBehavior::START_PLANNER); - velocity_factor_interface_.init(PlanningBehavior::START_PLANNER); } void StartPlannerModule::onFreespacePlannerTimer() @@ -732,9 +730,9 @@ BehaviorModuleOutput StartPlannerModule::plan() setDrivableAreaInfo(output); - setVelocityFactor(output.path); + set_longitudinal_planning_factor(output.path); - const auto steering_factor_direction = getSteeringFactorDirection(output); + const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { const double start_distance = autoware::motion_utils::calcSignedArcLength( @@ -744,9 +742,9 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, status_.pull_out_path.start_pose, + status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); setDebugData(); return output; } @@ -754,9 +752,9 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - steering_factor_direction, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + 0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, + planning_factor_direction, SafetyFactorArray{}); setDebugData(); @@ -839,7 +837,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() setDrivableAreaInfo(output); - const auto steering_factor_direction = getSteeringFactorDirection(output); + const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { const double start_distance = autoware::motion_utils::calcSignedArcLength( @@ -849,10 +847,9 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::APPROACHING, - ""); + planning_factor_interface_->add( + start_distance, finish_distance, status_.pull_out_path.start_pose, + status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); setDebugData(); return output; @@ -861,9 +858,9 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + 0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, + planning_factor_direction, SafetyFactorArray{}); setDebugData(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp index 895390f91cc16..6296acd8e71f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -41,7 +41,7 @@ class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 5848cc75f148e..247e22e1aedad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -46,7 +46,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface); CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; @@ -131,9 +132,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (finish_distance > -1.0e-03) { - steering_factor_interface_.set( - {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, - SteeringFactor::LEFT, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, left_shift.start_pose, left_shift.finish_pose, + PlanningFactor::SHIFT_LEFT, SafetyFactorArray{}); } } @@ -151,9 +152,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (finish_distance > -1.0e-03) { - steering_factor_interface_.set( - {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, - SteeringFactor::RIGHT, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, right_shift.start_pose, right_shift.finish_pose, + PlanningFactor::SHIFT_RIGHT, SafetyFactorArray{}); } } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 48729c9c4fa0c..739b5b21f59dc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -79,14 +79,13 @@ StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT helper_{std::make_shared(parameters)}, parameters_{parameters}, generator_{parameters} { - steering_factor_interface_.init(PlanningBehavior::AVOIDANCE); - velocity_factor_interface_.init(PlanningBehavior::AVOIDANCE); } bool StaticObstacleAvoidanceModule::isExecutionRequested() const @@ -776,7 +775,7 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior( insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); - setVelocityFactor(path.path); + set_longitudinal_planning_factor(path.path); } bool StaticObstacleAvoidanceModule::isSafePath( @@ -1209,13 +1208,12 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const output.start_distance_to_path_change = sl_front.start_longitudinal; output.finish_distance_to_path_change = sl_back.end_longitudinal; - const uint16_t steering_factor_direction = std::invoke([&output]() { - return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; + const uint16_t planning_factor_direction = std::invoke([&output]() { + return output.lateral_shift > 0.0 ? PlanningFactor::SHIFT_LEFT : PlanningFactor::SHIFT_RIGHT; }); - steering_factor_interface_.set( - {sl_front.start, sl_back.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + output.start_distance_to_path_change, output.finish_distance_to_path_change, sl_front.start, + sl_back.end, planning_factor_direction, SafetyFactorArray{}, true, 0.0, output.lateral_shift); output.path_candidate = shifted_path.path; return output; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 5e8ef9fc8a063..91eff0b01346a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -74,7 +74,9 @@ class BlindSpotModule : public SceneModuleInterfaceWithRTC BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 64e1435167a89..a978c074e0018 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -101,8 +101,11 @@ void BlindSpotModule::reactRTCApprovalByDecision( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, + "blind_spot(module is judging as UNSAFE)"); } return; } @@ -132,8 +135,11 @@ void BlindSpotModule::reactRTCApprovalByDecision( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, + "blind_spot(module is judging as SAFE and RTC is not approved)"); } return; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index d6cae9004600a..ee6516f025709 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -59,7 +59,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa registerModule(std::make_shared( module_id, lane_id, turn_direction, planner_data_, planner_param_, - logger_.get_child("blind_spot_module"), clock_)); + logger_.get_child("blind_spot_module"), clock_, time_keeper_, planning_factor_interface_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 8a9401646aaea..4fca5f821f934 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -40,14 +40,15 @@ namespace bg = boost::geometry; BlindSpotModule::BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_{planner_param}, turn_direction_(turn_direction), is_over_pass_judge_line_(false) { - velocity_factor_.init(PlanningBehavior::REAR_CHECK); sibling_straight_lanelet_ = getSiblingStraightLanelet( planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_), planner_data->route_handler_->getRoutingGraphPtr()); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index cdaff7225a7d2..5bfd31b75976a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -192,8 +192,8 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case // where both regulatory element and non-regulatory element crosswalks exist. registerModule(std::make_shared( - node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, - clock_)); + node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_, + time_keeper_, planning_factor_interface_)); generateUUID(crosswalk_lanelet_id); updateRTCStatus( getUUID(crosswalk_lanelet_id), true, State::WAITING_FOR_EXECUTION, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 64f7e9e14dcd1..844647e33f8c0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -34,12 +34,12 @@ #include #include #include +#include #include #include #include #include #include - namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -175,13 +175,14 @@ CrosswalkModule::CrosswalkModule( rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), planner_param_(planner_param), use_regulatory_element_(reg_elem_id) { - velocity_factor_.init(PlanningBehavior::CROSSWALK); passed_safety_slow_point_ = false; if (use_regulatory_element_) { @@ -898,9 +899,11 @@ void CrosswalkModule::applySlowDown( } } if (slowdown_pose) - velocity_factor_.set( - output.points, planner_data_->current_odometry->pose, *slowdown_pose, - VelocityFactor::APPROACHING); + planning_factor_interface_->add( + output.points, planner_data_->current_odometry->pose, *slowdown_pose, *slowdown_pose, + tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, + safety_slow_down_speed, 0.0 /*shift distance*/, "crosswalk_safety_slowdown_for_approaching"); } void CrosswalkModule::applySlowDownByLanelet2Map( @@ -1377,9 +1380,11 @@ void CrosswalkModule::planStop( // Plan stop insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path); - velocity_factor_.set( + planning_factor_interface_->add( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, - VelocityFactor::UNKNOWN); + stop_factor->stop_pose, tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, + 0.0 /*shift distance*/, "crosswalk_stop"); } bool CrosswalkModule::checkRestartSuppression( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d5a9e463c730b..f32ee96623822 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -334,7 +334,9 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index fb2c17d9e3745..f231d7adc3326 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -64,7 +64,8 @@ void DetectionAreaModuleManager::launchNewModules( if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, lane_id, *detection_area_with_lane_id.first, planner_param_, - logger_.get_child("detection_area_module"), clock_)); + logger_.get_child("detection_area_module"), clock_, time_keeper_, + planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 031c45753022e..59f6f20937f41 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -36,8 +36,10 @@ DetectionAreaModule::DetectionAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 9224cf4624687..1d3e5f3540ff3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -68,7 +68,9 @@ class DetectionAreaModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 41bdbe3e43c8b..d6f7dfbac92d4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -338,7 +338,8 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, node_, logger_.get_child("intersection_module"), clock_); + has_traffic_light, node_, logger_.get_child("intersection_module"), clock_, time_keeper_, + planning_factor_interface_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); @@ -526,7 +527,8 @@ void MergeFromPrivateModuleManager::launchNewModules( planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, - logger_.get_child("merge_from_private_road_module"), clock_)); + logger_.get_child("merge_from_private_road_module"), clock_, time_keeper_, + planning_factor_interface_)); continue; } } else { @@ -540,7 +542,8 @@ void MergeFromPrivateModuleManager::launchNewModules( planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, - logger_.get_child("merge_from_private_road_module"), clock_)); + logger_.get_child("merge_from_private_road_module"), clock_, time_keeper_, + planning_factor_interface_)); continue; } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 93a13a4d62737..2d5da0f070ed9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -18,7 +18,6 @@ #include // for toGeomPoly #include -#include #include #include // for toPolygon2d #include @@ -52,8 +51,10 @@ IntersectionModule::IntersectionModule( [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), lane_id_(lane_id), associative_ids_(associative_ids), @@ -61,8 +62,6 @@ IntersectionModule::IntersectionModule( has_traffic_light_(has_traffic_light), occlusion_uuid_(autoware::universe_utils::generateUUID()) { - velocity_factor_.init(PlanningBehavior::INTERSECTION); - { collision_state_machine_.setMarginTime( planner_param_.collision_detection.collision_detection_hold_time); @@ -705,7 +704,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); @@ -720,7 +719,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -734,7 +733,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -746,7 +745,8 @@ void reactRTCApprovalByDecisionResult( const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -760,9 +760,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(pure StuckStop)"); } } if (!rtc_occlusion_approved && decision_result.occlusion_stopline_idx) { @@ -771,9 +774,13 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(occlusion_stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(occlusion_stopline_idx).point.pose, + path->points.at(occlusion_stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(StuckStop with occlusion)"); } } return; @@ -785,7 +792,8 @@ void reactRTCApprovalByDecisionResult( const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -799,9 +807,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Yield Stuck)"); } } return; @@ -813,7 +824,8 @@ void reactRTCApprovalByDecisionResult( const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -825,9 +837,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop)"); } } if (!rtc_occlusion_approved) { @@ -836,9 +851,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } return; @@ -849,7 +867,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + tier4_planning_msgs::msg::PathWithLaneId * path, + motion_utils::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -862,9 +881,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_first_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with collision)"); } } if (!rtc_occlusion_approved) { @@ -881,9 +903,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with occlusion)"); } } return; @@ -894,7 +919,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + tier4_planning_msgs::msg::PathWithLaneId * path, + motion_utils::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -920,9 +946,13 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(occlusion_peeking_stopline).point.pose, VelocityFactor::UNKNOWN); + path->points.at(occlusion_peeking_stopline).point.pose, + path->points.at(occlusion_peeking_stopline).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(PeekingToOcclusion)"); } } if (!rtc_default_approved) { @@ -931,9 +961,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(PeekingToOcclusion while stopping for collision)"); } } return; @@ -945,7 +978,8 @@ void reactRTCApprovalByDecisionResult( const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -957,9 +991,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } if (!rtc_occlusion_approved) { @@ -972,9 +1009,12 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } return; @@ -986,7 +1026,8 @@ void reactRTCApprovalByDecisionResult( const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -998,9 +1039,13 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, + "intersection(Occlusion without traffic light, collision detected)"); } } if (!rtc_occlusion_approved && decision_result.temporal_stop_before_attention_required) { @@ -1009,9 +1054,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Occlusion without traffic light)"); } } if (!rtc_occlusion_approved && !decision_result.temporal_stop_before_attention_required) { @@ -1032,7 +1080,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1043,9 +1092,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for collision)"); } } if (!rtc_occlusion_approved) { @@ -1054,9 +1106,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for occlusion)"); } } return; @@ -1068,7 +1123,8 @@ void reactRTCApprovalByDecisionResult( const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1080,9 +1136,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FullyPrioritized, collision detected)"); } } if (!rtc_occlusion_approved) { @@ -1091,9 +1150,13 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, + "intersection(FullyPrioritized, RTC for occlusion is interrupting)"); } } return; @@ -1107,7 +1170,7 @@ void IntersectionModule::reactRTCApproval( VisitorSwitch{[&](const auto & decision) { reactRTCApprovalByDecisionResult( activated_, occlusion_activated_, decision, planner_param_, baselink2front, path, - &velocity_factor_, &debug_data_); + planning_factor_interface_.get(), &debug_data_); }}, decision_result); return; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index 6c31be2ce83b9..db0cfbe98d53c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -304,7 +304,9 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** *********************************************************** diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 3dfdcc36c0cff..fc442337198af 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -41,12 +41,13 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), associative_ids_(associative_ids) { - velocity_factor_.init(PlanningBehavior::MERGE); planner_param_ = planner_param; state_machine_.setState(StateMachine::State::STOP); } @@ -153,8 +154,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path) /* get stop point and stop factor */ const auto & stop_pose = path->points.at(stopline_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "merge_from_private"); const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength( path->points, current_pose.position, path->points.at(stopline_idx).point.pose.position); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index dc8bf1a96a7a2..fe446d304d9e0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -61,7 +61,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp index 9cb1153a8edd2..517568c93bd35 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -51,7 +51,8 @@ void NoDrivableLaneModuleManager::launchNewModules( } registerModule(std::make_shared( - module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_)); + module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_, + time_keeper_, planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index 8365251904b18..f32210bc502bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -21,14 +21,18 @@ #include #include +#include + namespace autoware::behavior_velocity_planner { using autoware::universe_utils::createPoint; NoDrivableLaneModule::NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_(planner_param), debug_data_(), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index d8c5e3e0425d1..835e946443e90 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -55,7 +55,9 @@ class NoDrivableLaneModule : public SceneModuleInterface NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index dca2dde33b693..55b6c0dffd1a4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -60,7 +60,7 @@ void NoStoppingAreaModuleManager::launchNewModules( // assign 1 no stopping area for each module registerModule(std::make_shared( module_id, lane_id, *m.first, planner_param_, logger_.get_child("no_stopping_area_module"), - clock_)); + clock_, time_keeper_, planning_factor_interface_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 5c79ec69a9d98..bcb8b365f6205 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -40,8 +41,10 @@ NoStoppingAreaModule::NoStoppingAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 1eafcf157623d..0d53441924805 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -57,7 +57,9 @@ class NoStoppingAreaModule : public SceneModuleInterfaceWithRTC const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index c4dace9d49244..d28cff55a7a8d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -123,8 +123,8 @@ void OcclusionSpotModuleManager::launchNewModules( // general if (!isModuleRegistered(module_id_)) { registerModule(std::make_shared( - module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), - clock_)); + module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), clock_, + time_keeper_, planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index c5d1cf61614b2..1163b00e08ccb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -64,8 +64,11 @@ namespace utils = occlusion_spot_utils; OcclusionSpotModule::OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), param_(planner_param) + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), + param_(planner_param) { velocity_factor_.init(PlanningBehavior::UNKNOWN); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 35c409a9c459b..22b2a91be66b0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -48,7 +48,9 @@ class OcclusionSpotModule : public SceneModuleInterface OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan occlusion spot velocity at unknown area in occupancy grid diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index a891b011dbab8..ad1ae1d47664a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -79,7 +80,9 @@ class SceneModuleInterface { public: explicit SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); virtual ~SceneModuleInterface() = default; virtual bool modifyPathVelocity(PathWithLaneId * path) = 0; @@ -94,13 +97,6 @@ class SceneModuleInterface planner_data_ = planner_data; } - void setTimeKeeper(const std::shared_ptr & time_keeper) - { - time_keeper_ = time_keeper; - } - - std::shared_ptr getTimeKeeper() { return time_keeper_; } - void resetVelocityFactor() { velocity_factor_.reset(); } VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } std::vector getObjectsOfInterestData() const { return objects_of_interest_; } @@ -111,9 +107,10 @@ class SceneModuleInterface rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - autoware::motion_utils::VelocityFactorInterface velocity_factor_; + autoware::motion_utils::VelocityFactorInterface velocity_factor_; // TODO(soblin): remove this std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; + std::shared_ptr planning_factor_interface_; void setObjectsOfInterestData( const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, @@ -148,6 +145,8 @@ class SceneModuleManagerInterface std::string("~/virtual_wall/") + module_name, 5); pub_velocity_factor_ = node.create_publisher( std::string("/planning/velocity_factors/") + module_name, 1); + planning_factor_interface_ = + std::make_shared(&node, module_name); processing_time_publisher_ = std::make_shared(&node, "~/debug"); @@ -191,6 +190,7 @@ class SceneModuleManagerInterface scene_module->modifyPathVelocity(path); // The velocity factor must be called after modifyPathVelocity. + // TODO(soblin): remove this const auto velocity_factor = scene_module->getVelocityFactor(); if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { velocity_factor_array.factors.emplace_back(velocity_factor); @@ -204,6 +204,7 @@ class SceneModuleManagerInterface } pub_velocity_factor_->publish(velocity_factor_array); + planning_factor_interface_->publish(); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { tier4_planning_msgs::msg::PathWithLaneId debug_path; @@ -247,7 +248,6 @@ class SceneModuleManagerInterface logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.emplace(scene_module->getModuleId()); - scene_module->setTimeKeeper(time_keeper_); scene_modules_.insert(scene_module); } @@ -282,6 +282,8 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_processing_time_detail_; std::shared_ptr time_keeper_; + + std::shared_ptr planning_factor_interface_; }; extern template SceneModuleManagerInterface::SceneModuleManagerInterface( rclcpp::Node & node, [[maybe_unused]] const char * module_name); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index ffd454012d13e..db2a274272f9f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -25,11 +25,14 @@ namespace autoware::behavior_velocity_planner { SceneModuleInterface::SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) : module_id_(module_id), logger_(logger), clock_(clock), - time_keeper_(std::shared_ptr()) + time_keeper_(time_keeper), + planning_factor_interface_(planning_factor_interface) { } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp index 4e30ab019aa4e..a955538f4f9fe 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -52,7 +52,9 @@ class SceneModuleInterfaceWithRTC : public SceneModuleInterface { public: explicit SceneModuleInterfaceWithRTC( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); virtual ~SceneModuleInterfaceWithRTC() = default; void setActivation(const bool activated) { activated_ = activated; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp index abac509fd2b2b..2e73b35e20ed9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -27,8 +27,10 @@ namespace autoware::behavior_velocity_planner { SceneModuleInterfaceWithRTC::SceneModuleInterfaceWithRTC( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), activated_(false), safe_(false), distance_(std::numeric_limits::lowest()) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index 4a16f263d0a54..4af7882461643 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -156,7 +156,8 @@ void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathW if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, planner_data_, planner_param_, logger_.get_child("run_out_module"), - std::move(dynamic_obstacle_creator_), debug_ptr_, clock_)); + std::move(dynamic_obstacle_creator_), debug_ptr_, clock_, time_keeper_, + planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index efc2d458ae2a7..193ba5eab0553 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -45,15 +45,15 @@ RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, - const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), debug_ptr_(debug_ptr), state_machine_(std::make_unique(planner_param.approaching.state)) { - velocity_factor_.init(PlanningBehavior::RUN_OUT); - if (planner_param.run_out.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); planning_utils::getAllPartitionLanelets(ll, partition_lanelets_); @@ -770,9 +770,10 @@ bool RunOutModule::insertStopPoint( stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); - velocity_factor_.set( - path.points, planner_data_->current_odometry->pose, stop_point.value(), VelocityFactor::UNKNOWN, - "run_out"); + planning_factor_interface_->add( + path.points, planner_data_->current_odometry->pose, stop_point.value(), stop_point.value(), + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift_distance*/, "run_out_stop"); return true; } @@ -876,8 +877,11 @@ void RunOutModule::insertApproachingVelocity( return; } - velocity_factor_.set( - output_path.points, current_pose, stop_point.value(), VelocityFactor::UNKNOWN, "run_out"); + planning_factor_interface_->add( + output_path.points, planner_data_->current_odometry->pose, stop_point.value(), + stop_point.value(), tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, + 0.0 /*shift_distance*/, "run_out_approaching_velocity"); // debug debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 83123c71f461e..49de16753a869 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -47,7 +47,9 @@ class RunOutModule : public SceneModuleInterface const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, - const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock); + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp index 4677d49b78f4b..a69e33e9e0391 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp @@ -63,7 +63,7 @@ void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, lane_id, *speed_bump_with_lane_id.first, planner_param_, - logger_.get_child("speed_bump_module"), clock_)); + logger_.get_child("speed_bump_module"), clock_, time_keeper_, planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp index 54ea807f3268b..4af7801e50ba3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -33,8 +34,10 @@ using geometry_msgs::msg::Point32; SpeedBumpModule::SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), lane_id_(lane_id), speed_bump_reg_elem_(std::move(speed_bump_reg_elem)), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index 176a01d5112c5..e3ee2c0566381 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -54,7 +55,9 @@ class SpeedBumpModule : public SceneModuleInterface SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index b305a1d2aa404..e289779df34b6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -81,7 +81,7 @@ void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pat if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, stop_line_with_lane_id.first, planner_param_, - logger_.get_child("stop_line_module"), clock_)); + logger_.get_child("stop_line_module"), clock_, time_keeper_, planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 2a9c5dab1ebd9..71a5ea3b9e807 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -20,6 +20,7 @@ #include +#include #include #include @@ -28,8 +29,10 @@ namespace autoware::behavior_velocity_planner StopLineModule::StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), stop_line_(std::move(stop_line)), planner_param_(planner_param), state_(State::APPROACH), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 9ac1463da8618..aac830b0e9f24 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -30,6 +30,7 @@ #include +#include #include #include @@ -68,7 +69,9 @@ class StopLineModule : public SceneModuleInterface StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp index 1eafb71eb403c..e304b479a005d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -74,7 +74,10 @@ class StopLineModuleTest : public ::testing::Test clock_ = std::make_shared(); module_ = std::make_shared( - 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_); + 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_, + std::make_shared(), + std::make_shared( + node_.get(), "test_stopline")); module_->setPlannerData(planner_data_); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp index 4325cc1d5aaf9..a004f5b823138 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp @@ -42,8 +42,9 @@ void TemplateModuleManager::launchNewModules( { int64_t module_id = 0; if (!isModuleRegistered(module_id)) { - registerModule( - std::make_shared(module_id, logger_.get_child(getModuleName()), clock_)); + registerModule(std::make_shared( + module_id, logger_.get_child(getModuleName()), clock_, time_keeper_, + planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index 3ce8502baaf63..d8eea337e7186 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -19,14 +19,17 @@ #include +#include #include namespace autoware::behavior_velocity_planner { TemplateModule::TemplateModule( - const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock) + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface) { } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index 70cd5cb1235e7..ba078f3fd6421 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -18,9 +18,9 @@ #include #include +#include #include #include - namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -29,7 +29,9 @@ class TemplateModule : public SceneModuleInterface { public: TemplateModule( - const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief Modify the velocity of path points. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 1b66824d04623..9a402f31af0e4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -114,7 +114,8 @@ void TrafficLightModuleManager::launchNewModules( if (!isModuleRegisteredFromExistingAssociatedModule(lane_id)) { registerModule(std::make_shared( lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, - logger_.get_child("traffic_light_module"), clock_)); + logger_.get_child("traffic_light_module"), clock_, time_keeper_, + planning_factor_interface_)); generateUUID(lane_id); updateRTCStatus( getUUID(lane_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 458d8e1588a5b..b051d5ff7eb76 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -43,8 +43,10 @@ namespace autoware::behavior_velocity_planner TrafficLightModule::TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(lane_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 3af13bc1927ce..c30cbbdfc1477 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -69,7 +69,9 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 3815c83d3b6ab..dade98dfc1133 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -94,7 +94,8 @@ void VirtualTrafficLightModuleManager::launchNewModules( ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) { registerModule(std::make_shared( module_id, lane_id, *m.first, m.second, planner_param_, - logger_.get_child("virtual_traffic_light_module"), clock_)); + logger_.get_child("virtual_traffic_light_module"), clock_, time_keeper_, + planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 86239816ed6f2..9ba7b0bccf1b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -38,8 +39,10 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), reg_elem_(reg_elem), lane_(lane), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 7f37e7078a431..9e7a1192d9869 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -31,6 +31,7 @@ #include #include +#include #include #include @@ -81,7 +82,9 @@ class VirtualTrafficLightModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp index 0f1a7509043b5..bfa8a96531090 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -61,7 +61,8 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); + lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_, time_keeper_, + planning_factor_interface_)); }; const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 6a7505930a334..94421ae27e077 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -32,15 +33,15 @@ using autoware::universe_utils::getPose; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), state_(State::APPROACH), planner_param_(planner_param), use_regulatory_element_(use_regulatory_element) { - velocity_factor_.init(PlanningBehavior::SIDEWALK); - if (use_regulatory_element_) { const auto reg_elem_ptr = std::dynamic_pointer_cast( lanelet_map_ptr->regulatoryElementLayer.get(module_id)); @@ -119,9 +120,10 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path) } /* get stop point and stop factor */ - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose.value(), - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose.value(), stop_pose.value(), + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift distance*/, "walkway_stop"); // use arc length to identify if ego vehicle is in front of walkway stop or not. const double signed_arc_dist_to_stop_point = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index df54eafd11cc2..1bd5003498d34 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -44,7 +44,9 @@ class WalkwayModule : public SceneModuleInterface WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 7fd5834ba2cfa..2000615b51e2c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -43,7 +43,9 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo module_name_ = module_name; logger_ = node.get_logger().get_child(ns_); clock_ = node.get_clock(); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + planning_factor_interface_ = std::make_unique( + &node, "dynamic_obstacle_stop"); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -160,15 +162,9 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( debug_data_.stop_pose = stop_pose; if (stop_pose) { result.stop_points.push_back(stop_pose->position); - const auto stop_pose_reached = - planner_data->current_odometry.twist.twist.linear.x < 1e-3 && - autoware::universe_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; - velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, *stop_pose, - stop_pose_reached ? autoware::motion_utils::VelocityFactor::STOPPED - : autoware::motion_utils::VelocityFactor::APPROACHING, - "dynamic_obstacle_stop"); - result.velocity_factor = velocity_factor_interface_.get(); + planning_factor_interface_->add( + ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP, + SafetyFactorArray{}); create_virtual_walls(); } } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp index 1843d26a22a29..82a6d790bebe7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp @@ -33,6 +33,7 @@ class DynamicObstacleStopModule : public PluginModuleInterface { public: void init(rclcpp::Node & node, const std::string & module_name) override; + void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( const std::vector & ego_trajectory_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 1923f023552e3..eb89c027a3b48 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -48,7 +48,6 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string projection_params_ = obstacle_velocity_limiter::ProjectionParameters(node); obstacle_params_ = obstacle_velocity_limiter::ObstacleParameters(node); velocity_params_ = obstacle_velocity_limiter::VelocityParameters(node); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index bd85546cb6c56..532cc1e8c7d51 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -58,7 +58,9 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) logger_ = node.get_logger(); clock_ = node.get_clock(); init_parameters(node); - velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + planning_factor_interface_ = + std::make_unique(&node, "out_of_lane"); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -310,15 +312,9 @@ VelocityPlanningResult OutOfLaneModule::plan( slowdown_pose->position, slowdown_pose->position, slowdown_velocity); } - const auto is_approaching = - motion_utils::calcSignedArcLength( - ego_trajectory_points, ego_data.pose.position, slowdown_pose->position) > 0.1 && - planner_data->current_odometry.twist.twist.linear.x > 0.1; - const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING - : motion_utils::VelocityFactor::STOPPED; - velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, *slowdown_pose, status, "out_of_lane"); - result.velocity_factor = velocity_factor_interface_.get(); + planning_factor_interface_->add( + ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}); virtual_wall_marker_creator.add_virtual_walls( out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_)); virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index 72f20f1c63d96..0e73994dd73a7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -38,6 +38,7 @@ class OutOfLaneModule : public PluginModuleInterface { public: void init(rclcpp::Node & node, const std::string & module_name) override; + void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( const std::vector & ego_trajectory_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index 53860c390b4db..6d683acff667f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -18,7 +18,7 @@ #include "planner_data.hpp" #include "velocity_planning_result.hpp" -#include +#include #include #include @@ -32,6 +32,9 @@ namespace autoware::motion_velocity_planner { +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; + class PluginModuleInterface { public: @@ -42,7 +45,7 @@ class PluginModuleInterface const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; - autoware::motion_utils::VelocityFactorInterface velocity_factor_interface_; + virtual void publish_planning_factor() {} rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; @@ -50,6 +53,9 @@ class PluginModuleInterface rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; + +protected: + std::unique_ptr planning_factor_interface_; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp index 1288884a37979..7b41bf0ee9e3e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp @@ -17,7 +17,6 @@ #include -#include #include #include @@ -43,7 +42,6 @@ struct VelocityPlanningResult { std::vector stop_points{}; std::vector slowdown_intervals{}; - std::optional velocity_factor{}; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md index 01062f81e77a2..c65593497b8d5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -30,10 +30,10 @@ This means that to stop before a wall, a stop point is inserted in the trajector ## Output topics -| Name | Type | Description | -| --------------------------- | ------------------------------------------------- | -------------------------------------------------- | -| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | -| `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile | +| Name | Type | Description | +| ----------------------------------------- | ---------------------------------------------- | -------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/planning_factors/` | tier4_planning_msgs::msg::PlanningFactorsArray | factors causing change in the ego velocity profile | ## Services diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index a78ab1489e080..8f5aa761573b7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -82,9 +82,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Publishers trajectory_pub_ = this->create_publisher("~/output/trajectory", 1); - velocity_factor_publisher_ = - this->create_publisher( - "~/output/velocity_factors", 1); processing_time_publisher_ = this->create_publisher( "~/debug/processing_time_ms", 1); @@ -422,20 +419,13 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj resampled_trajectory, std::make_shared(planner_data_)); processing_times["plan_velocities"] = stop_watch.toc("plan_velocities"); - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; - velocity_factors.header.frame_id = "map"; - velocity_factors.header.stamp = get_clock()->now(); - for (const auto & planning_result : planning_results) { for (const auto & stop_point : planning_result.stop_points) insert_stop(output_trajectory_msg, stop_point); for (const auto & slowdown_interval : planning_result.slowdown_intervals) insert_slowdown(output_trajectory_msg, slowdown_interval); - if (planning_result.velocity_factor) - velocity_factors.factors.push_back(*planning_result.velocity_factor); } - velocity_factor_publisher_->publish(velocity_factors); return output_trajectory_msg; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 18d41f5d7fe5d..4235971e76fca 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -93,8 +93,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node // publishers rclcpp::Publisher::SharedPtr trajectory_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr - velocity_factor_publisher_; autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; rclcpp::Publisher::SharedPtr diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index b08798cbef772..5bdf386479283 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -102,6 +102,8 @@ std::vector MotionVelocityPlannerManager::plan_velocitie VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); results.push_back(res); + plugin->publish_planning_factor(); + if (res.stop_points.size() > 0) { const auto stop_decision_metric = make_decision_metric(plugin->get_module_name(), "stop"); metrics_.push_back(stop_decision_metric); diff --git a/system/autoware_default_adapi/package.xml b/system/autoware_default_adapi/package.xml index a24d0c8a30b62..e0d3fc0f9e21b 100644 --- a/system/autoware_default_adapi/package.xml +++ b/system/autoware_default_adapi/package.xml @@ -32,6 +32,7 @@ std_srvs tier4_api_msgs tier4_control_msgs + tier4_planning_msgs tier4_system_msgs python3-flask diff --git a/system/autoware_default_adapi/src/planning.cpp b/system/autoware_default_adapi/src/planning.cpp index d89ef6c221666..e11b01115caad 100644 --- a/system/autoware_default_adapi/src/planning.cpp +++ b/system/autoware_default_adapi/src/planning.cpp @@ -18,6 +18,7 @@ #include +#include #include #include #include @@ -25,6 +26,34 @@ namespace autoware::default_adapi { +const std::map direction_map = { + {PlanningFactor::SHIFT_RIGHT, SteeringFactor::RIGHT}, + {PlanningFactor::SHIFT_LEFT, SteeringFactor::LEFT}, + {PlanningFactor::TURN_RIGHT, SteeringFactor::RIGHT}, + {PlanningFactor::TURN_LEFT, SteeringFactor::LEFT}}; + +const std::map conversion_map = { + {"behavior_path_planner", PlanningBehavior::INTERSECTION}, + {"static_obstacle_avoidance", PlanningBehavior::AVOIDANCE}, + {"crosswalk", PlanningBehavior::CROSSWALK}, + {"goal_planner", PlanningBehavior::GOAL_PLANNER}, + {"intersection", PlanningBehavior::INTERSECTION}, + {"lane_change_left", PlanningBehavior::LANE_CHANGE}, + {"lane_change_right", PlanningBehavior::LANE_CHANGE}, + {"merge_from_private", PlanningBehavior::MERGE}, + {"no_stopping_area", PlanningBehavior::NO_STOPPING_AREA}, + {"blind_spot", PlanningBehavior::REAR_CHECK}, + {"obstacle_cruise_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"motion_velocity_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"walkway", PlanningBehavior::SIDEWALK}, + {"start_planner", PlanningBehavior::START_PLANNER}, + {"stop_line", PlanningBehavior::STOP_SIGN}, + {"surround_obstacle_checker", PlanningBehavior::SURROUNDING_OBSTACLE}, + {"traffic_light", PlanningBehavior::TRAFFIC_SIGNAL}, + {"detection_area", PlanningBehavior::USER_DEFINED_DETECTION_AREA}, + {"virtual_traffic_light", PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT}, + {"run_out", PlanningBehavior::RUN_OUT}}; + template void concat(std::vector & v1, const std::vector & v2) { @@ -49,16 +78,122 @@ std::vector::SharedPtr> init_factors( } template -T merge_factors(const rclcpp::Time stamp, const std::vector & factors) +std::vector convert([[maybe_unused]] const std::vector & factors) +{ + static_assert(sizeof(T) == 0, "Only specializations of convert can be used."); + throw std::logic_error("Only specializations of convert can be used."); +} + +template <> +std::vector convert(const std::vector & factors) +{ + std::vector velocity_factors; + + for (const auto & factor : factors) { + if (factor.behavior != PlanningFactor::SLOW_DOWN && factor.behavior != PlanningFactor::STOP) { + continue; + } + + if (factor.control_points.empty()) { + continue; + } + + if (conversion_map.count(factor.module) == 0) { + continue; + } + + VelocityFactor velocity_factor; + velocity_factor.behavior = conversion_map.at(factor.module); + velocity_factor.pose = factor.control_points.front().pose; + velocity_factor.distance = factor.control_points.front().distance; + + velocity_factors.push_back(velocity_factor); + } + + return velocity_factors; +} + +template <> +std::vector convert(const std::vector & factors) { - T message; + std::vector steering_factors; + + for (const auto & factor : factors) { + if ( + factor.behavior != PlanningFactor::SHIFT_RIGHT && + factor.behavior != PlanningFactor::SHIFT_LEFT && + factor.behavior != PlanningFactor::TURN_RIGHT && + factor.behavior != PlanningFactor::TURN_LEFT) { + continue; + } + + if (factor.control_points.size() < 2) { + continue; + } + + if (conversion_map.count(factor.module) == 0) { + continue; + } + + if (direction_map.count(factor.behavior) == 0) { + continue; + } + + SteeringFactor steering_factor; + steering_factor.behavior = conversion_map.at(factor.module); + steering_factor.direction = direction_map.at(factor.behavior); + steering_factor.pose = std::array{ + factor.control_points.front().pose, factor.control_points.back().pose}; + steering_factor.distance = std::array{ + factor.control_points.front().distance, factor.control_points.back().distance}; + + steering_factors.push_back(steering_factor); + } + + return steering_factors; +} + +template +T merge_factors( + [[maybe_unused]] const rclcpp::Time stamp, + [[maybe_unused]] const std::vector & factors) +{ + static_assert(sizeof(T) == 0, "Only specializations of merge_factors can be used."); + throw std::logic_error("Only specializations of merge_factors can be used."); +} + +template <> +VelocityFactorArray merge_factors( + const rclcpp::Time stamp, const std::vector & factors) +{ + VelocityFactorArray message; message.header.stamp = stamp; message.header.frame_id = "map"; for (const auto & factor : factors) { - if (factor) { - concat(message.factors, factor->factors); + if (!factor) { + continue; } + + concat(message.factors, convert(factor->factors)); + } + return message; +} + +template <> +SteeringFactorArray merge_factors( + const rclcpp::Time stamp, const std::vector & factors) +{ + SteeringFactorArray message; + message.header.stamp = stamp; + message.header.frame_id = "map"; + + for (const auto & factor : factors) { + if (!factor) { + continue; + } + + concat(message.factors, convert(factor->factors)); } return message; } @@ -70,46 +205,33 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning stop_duration_ = declare_parameter("stop_duration", 1.0); stop_checker_ = std::make_unique(this, stop_duration_ + 1.0); - std::vector velocity_factor_topics = { - "/planning/velocity_factors/blind_spot", - "/planning/velocity_factors/crosswalk", - "/planning/velocity_factors/detection_area", - "/planning/velocity_factors/dynamic_obstacle_stop", - "/planning/velocity_factors/intersection", - "/planning/velocity_factors/merge_from_private", - "/planning/velocity_factors/no_stopping_area", - "/planning/velocity_factors/obstacle_stop", - "/planning/velocity_factors/obstacle_cruise", - "/planning/velocity_factors/occlusion_spot", - "/planning/velocity_factors/run_out", - "/planning/velocity_factors/stop_line", - "/planning/velocity_factors/surround_obstacle", - "/planning/velocity_factors/traffic_light", - "/planning/velocity_factors/virtual_traffic_light", - "/planning/velocity_factors/walkway", - "/planning/velocity_factors/motion_velocity_planner", - "/planning/velocity_factors/static_obstacle_avoidance", - "/planning/velocity_factors/dynamic_obstacle_avoidance", - "/planning/velocity_factors/avoidance_by_lane_change", - "/planning/velocity_factors/lane_change_left", - "/planning/velocity_factors/lane_change_right", - "/planning/velocity_factors/start_planner", - "/planning/velocity_factors/goal_planner"}; - - std::vector steering_factor_topics = { - "/planning/steering_factor/static_obstacle_avoidance", - "/planning/steering_factor/dynamic_obstacle_avoidance", - "/planning/steering_factor/avoidance_by_lane_change", - "/planning/steering_factor/intersection", - "/planning/steering_factor/lane_change_left", - "/planning/steering_factor/lane_change_right", - "/planning/steering_factor/start_planner", - "/planning/steering_factor/goal_planner"}; - - sub_velocity_factors_ = - init_factors(this, velocity_factors_, velocity_factor_topics); - sub_steering_factors_ = - init_factors(this, steering_factors_, steering_factor_topics); + std::vector factor_topics = { + "/planning/planning_factors/behavior_path_planner", + "/planning/planning_factors/blind_spot", + "/planning/planning_factors/crosswalk", + "/planning/planning_factors/detection_area", + "/planning/planning_factors/dynamic_obstacle_stop", + "/planning/planning_factors/intersection", + "/planning/planning_factors/merge_from_private", + "/planning/planning_factors/no_stopping_area", + "/planning/planning_factors/obstacle_cruise_planner", + "/planning/planning_factors/occlusion_spot", + "/planning/planning_factors/run_out", + "/planning/planning_factors/stop_line", + "/planning/planning_factors/surround_obstacle_checker", + "/planning/planning_factors/traffic_light", + "/planning/planning_factors/virtual_traffic_light", + "/planning/planning_factors/walkway", + "/planning/planning_factors/motion_velocity_planner", + "/planning/planning_factors/static_obstacle_avoidance", + "/planning/planning_factors/dynamic_obstacle_avoidance", + "/planning/planning_factors/avoidance_by_lane_change", + "/planning/planning_factors/lane_change_left", + "/planning/planning_factors/lane_change_right", + "/planning/planning_factors/start_planner", + "/planning/planning_factors/goal_planner"}; + + sub_factors_ = init_factors(this, factors_, factor_topics); const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this); adaptor.init_pub(pub_velocity_factors_); @@ -139,8 +261,8 @@ void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg) void PlanningNode::on_timer() { using autoware_adapi_v1_msgs::msg::VelocityFactor; - auto velocity = merge_factors(now(), velocity_factors_); - auto steering = merge_factors(now(), steering_factors_); + auto velocity = merge_factors(now(), factors_); + auto steering = merge_factors(now(), factors_); // Set the distance if it is nan. if (trajectory_ && kinematic_state_) { @@ -164,6 +286,13 @@ void PlanningNode::on_timer() } } + for (auto & factor : steering.factors) { + if ((factor.status == SteeringFactor::UNKNOWN) && (!std::isnan(factor.distance.front()))) { + const auto is_turning = factor.distance.front() < 0.0; + factor.status = is_turning ? SteeringFactor::TURNING : SteeringFactor::APPROACHING; + } + } + pub_velocity_factors_->publish(velocity); pub_steering_factors_->publish(steering); } diff --git a/system/autoware_default_adapi/src/planning.hpp b/system/autoware_default_adapi/src/planning.hpp index 3a9b99e33a997..49584475734f6 100644 --- a/system/autoware_default_adapi/src/planning.hpp +++ b/system/autoware_default_adapi/src/planning.hpp @@ -21,7 +21,11 @@ #include #include +#include +#include + #include +#include #include // This file should be included after messages. @@ -30,22 +34,26 @@ namespace autoware::default_adapi { +using autoware_adapi_v1_msgs::msg::PlanningBehavior; +using autoware_adapi_v1_msgs::msg::SteeringFactor; +using autoware_adapi_v1_msgs::msg::SteeringFactorArray; +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; + class PlanningNode : public rclcpp::Node { public: explicit PlanningNode(const rclcpp::NodeOptions & options); private: - using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; - using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; Pub pub_velocity_factors_; Pub pub_steering_factors_; Sub sub_trajectory_; Sub sub_kinematic_state_; - std::vector::SharedPtr> sub_velocity_factors_; - std::vector::SharedPtr> sub_steering_factors_; - std::vector velocity_factors_; - std::vector steering_factors_; + std::vector::SharedPtr> sub_factors_; + std::vector factors_; rclcpp::TimerBase::SharedPtr timer_; using VehicleStopChecker = autoware::motion_utils::VehicleStopCheckerBase; From dd632fc2a2bec57796ea1c0bfbfa9b7dafc93749 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 16 Jan 2025 22:45:07 +0900 Subject: [PATCH 36/59] feat(motion_utils): add detail and pass type to VirtualWall (#9940) Signed-off-by: Mamoru Sobue --- .../motion_utils/marker/marker_helper.hpp | 5 ++ .../marker/virtual_wall_marker_creator.hpp | 3 +- .../src/marker/marker_helper.cpp | 49 ++++++++++++++++++- .../marker/virtual_wall_marker_creator.cpp | 10 +++- 4 files changed, 63 insertions(+), 4 deletions(-) diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp index ee642c93a378f..2b89fc19d1878 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp @@ -40,6 +40,11 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", const bool is_driving_forward = true); +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward); + visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp index 07fcbd9840c88..fd86c54d65be9 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -29,12 +29,13 @@ namespace autoware::motion_utils { /// @brief type of virtual wall associated with different marker styles and namespace -enum VirtualWallType { stop, slowdown, deadline }; +enum VirtualWallType { stop, slowdown, deadline, pass }; /// @brief virtual wall to be visualized in rviz struct VirtualWall { geometry_msgs::msg::Pose pose{}; std::string text{}; + std::string detail{}; std::string ns{}; VirtualWallType style = stop; double longitudinal_offset{}; diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp index 91ad1c41eecc3..44b621a53c5f1 100644 --- a/common/autoware_motion_utils/src/marker/marker_helper.cpp +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -54,7 +54,7 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( { auto marker = createDefaultMarker( "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(0.0, 0.0, 1.0 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); marker.pose = vehicle_front_pose; marker.pose.position.z += 2.0; @@ -85,6 +85,41 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( return marker_array; } + +inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray( + const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Arrow + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW, + createMarkerScale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color); + + marker.pose = vehicle_front_pose; + + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.4 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 2.0; + marker.text = module_name; + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + } // namespace namespace autoware::motion_utils @@ -125,6 +160,18 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( createMarkerColor(0.0, 1.0, 0.0, 0.5)); } +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + return createIntendedPassArrowMarkerArray( + pose_with_offset, module_name, ns_prefix + "intended_pass_", now, id, + createMarkerColor(0.77, 0.77, 0.77, 0.5)); +} + visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id) { diff --git a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp index 4fecaea1bb838..fb8708b5b7baa 100644 --- a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -63,10 +63,16 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( case deadline: create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker; break; + case pass: + create_fn = autoware::motion_utils::createIntendedPassVirtualMarker; + break; } + const auto wall_text = virtual_wall.detail.empty() + ? virtual_wall.text + : virtual_wall.text + "(" + virtual_wall.detail + ")"; auto markers = create_fn( - virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, - virtual_wall.ns, virtual_wall.is_driving_forward); + virtual_wall.pose, wall_text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns, + virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { marker.id = static_cast(marker_count_per_namespace_[marker.ns].current++); marker_array.markers.push_back(marker); From 62e07a18c6c54f3a0213bceae320bb9e56e8af2a Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Thu, 16 Jan 2025 23:12:30 +0900 Subject: [PATCH 37/59] fix(start_planner, goal_planner): refactor lane departure checker initialization (#9944) --- .../src/pull_over_planner/geometric_pull_over.cpp | 8 ++++++-- .../src/pull_over_planner/shift_pull_over.cpp | 8 ++++++-- .../src/geometric_pull_out.cpp | 6 ++++-- .../src/shift_pull_out.cpp | 6 ++++-- 4 files changed, 20 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index 44dca13ccfd66..b7b4ad83de362 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -29,8 +29,12 @@ GeometricPullOver::GeometricPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward) : PullOverPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_{ - lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, is_forward_{is_forward}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 8350252369cc6..c8e0788e39b6f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -32,8 +32,12 @@ namespace autoware::behavior_path_planner { ShiftPullOver::ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, - lane_departure_checker_{ - lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 69c5c41405932..ed09e0c0a33d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -40,10 +40,12 @@ GeometricPullOut::GeometricPullOut( : PullOutPlannerBase{node, parameters, time_keeper}, parallel_parking_parameters_{parameters.parallel_parking_parameters} { + auto lane_departure_checker_params = autoware::lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; lane_departure_checker_ = std::make_shared( - autoware::lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, - vehicle_info_, time_keeper_); + lane_departure_checker_params, vehicle_info_); planner_.setParameters(parallel_parking_parameters_); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 8f4de1d3636de..16a3d4ffd842a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -44,10 +44,12 @@ ShiftPullOut::ShiftPullOut( std::shared_ptr time_keeper) : PullOutPlannerBase{node, parameters, time_keeper} { + autoware::lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; lane_departure_checker_ = std::make_shared( - autoware::lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, - vehicle_info_, time_keeper_); + lane_departure_checker_params, vehicle_info_, time_keeper_); } std::optional ShiftPullOut::plan( From 0bf689c2dc5a39dbe8749f270d5c21ec4733e56a Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 16 Jan 2025 23:39:33 +0900 Subject: [PATCH 38/59] refactor(lane_change): add missing safety check parameter (#9928) * refactor(lane_change): parameterize incoming object yaw threshold Signed-off-by: Zulfaqar Azmi * Readme Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * Add missing parameters Signed-off-by: Zulfaqar Azmi * missing dot Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> * update readme Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- .../README.md | 7 +++- .../config/lane_change.param.yaml | 7 +++- .../structs/parameters.hpp | 1 + .../src/manager.cpp | 32 ++++++++++++++++++- .../src/scene.cpp | 8 +++-- 5 files changed, 49 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 20fd564049133..601fb893f82c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -1149,7 +1149,8 @@ The following parameters are used to configure terminal lane change path feature | `collision_check.check_other_lanes` | [-] | boolean | If true, the lane change module includes objects in other lanes when performing collision assessment. | false | | `collision_check.use_all_predicted_paths` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | | `collision_check.prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | +| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between predicted ego pose and predicted object pose when executing rss-based collision checking | 3.1416 | +| `collision_check.th_incoming_object_yaw` | [rad] | double | Maximum yaw difference between current ego pose and current object pose. Objects with a yaw difference exceeding this value are excluded from the safety check. | 2.3562 | #### safety constraints during lane change path is computed @@ -1162,6 +1163,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.execution.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.execution.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | #### safety constraints specifically for stopped or parked vehicles @@ -1174,6 +1176,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.parked.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | | `safety_check.parked.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.parked.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.parked.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | ##### safety constraints to cancel lane change path @@ -1186,6 +1189,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | | `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 2.5 | | `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.6 | +| `safety_check.cancel.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | ##### safety constraints used during lane change path is computed when ego is stuck @@ -1198,6 +1202,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.stuck.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.stuck.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | (\*1) the value must be negative. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 15eb23daecf95..71fac1246598f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -58,6 +58,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" parked: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -66,6 +67,7 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" cancel: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -74,6 +76,7 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 2.5 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" stuck: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 @@ -82,6 +85,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" # lane expansion for object filtering lane_expansion: @@ -123,7 +127,8 @@ intersection: true turns: true prediction_time_resolution: 0.5 - yaw_diff_threshold: 3.1416 + th_incoming_object_yaw: 2.3562 # [rad] + yaw_diff_threshold: 3.1416 # [rad] check_current_lanes: false check_other_lanes: false use_all_predicted_paths: false diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index 60386f535fc64..358f60f3193d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -94,6 +94,7 @@ struct CollisionCheckParameters bool check_current_lane{true}; bool check_other_lanes{true}; bool use_all_predicted_paths{false}; + double th_incoming_object_yaw{2.3562}; double th_yaw_diff{3.1416}; double prediction_time_resolution{0.5}; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 450497fb2ca29..589338d7b3ace 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -152,6 +152,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("collision_check.prediction_time_resolution")); p.safety.collision_check.th_yaw_diff = getOrDeclareParameter(*node, parameter("collision_check.yaw_diff_threshold")); + p.safety.collision_check.th_incoming_object_yaw = + getOrDeclareParameter(*node, parameter("collision_check.th_incoming_object_yaw")); // rss check auto set_rss_params = [&](auto & params, const std::string & prefix) { @@ -169,6 +171,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s *node, parameter(prefix + ".rear_vehicle_safety_time_margin")); params.lateral_distance_max_threshold = getOrDeclareParameter(*node, parameter(prefix + ".lateral_distance_max_threshold")); + params.extended_polygon_policy = + getOrDeclareParameter(*node, parameter(prefix + ".extended_polygon_policy")); }; set_rss_params(p.safety.rss_params, "safety_check.execution"); set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked"); @@ -454,6 +458,19 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorsafety.collision_check.prediction_time_resolution); updateParam( parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff); + + auto th_incoming_object_yaw = p->safety.collision_check.th_incoming_object_yaw; + updateParam(parameters, ns + "th_incoming_object_yaw", th_incoming_object_yaw); + if (th_incoming_object_yaw >= M_PI_2) { + p->safety.collision_check.th_incoming_object_yaw = th_incoming_object_yaw; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, + "The value of th_incoming_object_yaw (%.3f rad) is less than the minimum possible value " + "(%.3f " + "rad).", + th_incoming_object_yaw, M_PI_2); + } } { @@ -483,7 +500,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "stop_time", p->th_stop_time); } - auto update_rss_params = [¶meters](const std::string & prefix, auto & params) { + auto update_rss_params = [¶meters, this](const std::string & prefix, auto & params) { using autoware::universe_utils::updateParam; updateParam( parameters, prefix + "longitudinal_distance_min_threshold", @@ -502,6 +519,19 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold); + + auto extended_polygon_policy = params.extended_polygon_policy; + updateParam( + parameters, prefix + "extended_polygon_policy", extended_polygon_policy); + if (extended_polygon_policy == "rectangle" || extended_polygon_policy == "along_path") { + params.extended_polygon_policy = extended_polygon_policy; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Policy %s not supported or there's typo. Make sure you choose either 'rectangle' or " + "'along_path'", + extended_polygon_policy.c_str()); + } }; update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 45fcd6fa99086..611a0b60a4366 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1051,7 +1051,9 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const const auto is_same_direction = [&](const PredictedObject & object) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - return !utils::path_safety_checker::isTargetObjectOncoming(current_pose, object_pose); + return !utils::path_safety_checker::isTargetObjectOncoming( + current_pose, object_pose, + common_data_ptr_->lc_param_ptr->safety.collision_check.th_incoming_object_yaw); }; // Perception noise could make stationary objects seem opposite the ego vehicle; check the @@ -1792,7 +1794,6 @@ bool NormalLaneChange::is_colliding( constexpr auto is_safe{true}; auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); - constexpr auto collision_check_yaw_diff_threshold{M_PI}; constexpr auto hysteresis_factor{1.0}; const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->safety.collision_check.use_all_predicted_paths); @@ -1817,7 +1818,8 @@ bool NormalLaneChange::is_colliding( const auto collided_polygons = utils::path_safety_checker::get_collided_polygons( lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param.vehicle_info, selected_rss_param, hysteresis_factor, safety_check_max_vel, - collision_check_yaw_diff_threshold, current_debug_data.second); + common_data_ptr_->lc_param_ptr->safety.collision_check.th_yaw_diff, + current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( From 67ab350f6e1af0c14bee74d6913c99165c2e9262 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 17 Jan 2025 00:05:32 +0900 Subject: [PATCH 39/59] feat(behavior_velocity_planner)!: remove velocity_factor completely (#9943) * feat(behavior_velocity_planner)!: remove velocity_factor completely Signed-off-by: Mamoru Sobue * minimize diff Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../src/scene.cpp | 8 +++--- .../src/scene_intersection.cpp | 2 -- .../src/scene.cpp | 19 +++++++++----- .../src/scene_no_stopping_area.cpp | 9 ++++--- .../src/scene_occlusion_spot.cpp | 2 -- .../scene_module_interface.hpp | 22 ---------------- .../src/scene.cpp | 26 +++++-------------- .../src/scene.hpp | 5 ---- .../src/manager.cpp | 12 --------- .../src/scene.cpp | 7 ++--- .../src/scene.cpp | 15 ++++++----- .../src/scene.hpp | 4 --- 12 files changed, 39 insertions(+), 92 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 59f6f20937f41..7e6a320f3ddad 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -46,7 +46,6 @@ DetectionAreaModule::DetectionAreaModule( planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) @@ -180,9 +179,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); } return true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 2d5da0f070ed9..5b8aca1c8ec9d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -44,8 +44,6 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using autoware::motion_utils::VelocityFactorInterface; - IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index f32210bc502bf..0e646e92035cf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -38,7 +38,6 @@ NoDrivableLaneModule::NoDrivableLaneModule( debug_data_(), state_(State::INIT) { - velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE); } bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path) @@ -166,8 +165,10 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = op_stop_pose.value(); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -214,8 +215,10 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * // Get stop point and stop factor { const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto & virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -252,8 +255,10 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = ego_pos_on_path.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index bcb8b365f6205..c788a54073ed6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -50,7 +50,6 @@ NoStoppingAreaModule::NoStoppingAreaModule( planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA); state_machine_.setState(StateMachine::State::GO); state_machine_.setMarginTime(planner_param_.state_clear_time); } @@ -144,9 +143,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_point->second, stop_point->second, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); } } else if (state_machine_.getState() == StateMachine::State::GO) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 1163b00e08ccb..fa259b2bea9aa 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -70,8 +70,6 @@ OcclusionSpotModule::OcclusionSpotModule( : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), param_(planner_param) { - velocity_factor_.init(PlanningBehavior::UNKNOWN); - if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; //! occupancy grid limitation( 100 * 100 ) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index ad1ae1d47664a..5bff3ca343e72 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -28,8 +27,6 @@ #include #include -#include -#include #include #include #include @@ -51,8 +48,6 @@ namespace autoware::behavior_velocity_planner { -using autoware::motion_utils::PlanningBehavior; -using autoware::motion_utils::VelocityFactor; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::universe_utils::DebugPublisher; @@ -97,8 +92,6 @@ class SceneModuleInterface planner_data_ = planner_data; } - void resetVelocityFactor() { velocity_factor_.reset(); } - VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } std::vector getObjectsOfInterestData() const { return objects_of_interest_; } void clearObjectsOfInterestData() { objects_of_interest_.clear(); } @@ -107,7 +100,6 @@ class SceneModuleInterface rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - autoware::motion_utils::VelocityFactorInterface velocity_factor_; // TODO(soblin): remove this std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; std::shared_ptr planning_factor_interface_; @@ -143,8 +135,6 @@ class SceneModuleManagerInterface } pub_virtual_wall_ = node.create_publisher( std::string("~/virtual_wall/") + module_name, 5); - pub_velocity_factor_ = node.create_publisher( - std::string("/planning/velocity_factors/") + module_name, 1); planning_factor_interface_ = std::make_shared(&node, module_name); @@ -180,21 +170,12 @@ class SceneModuleManagerInterface StopWatch stop_watch; stop_watch.tic("Total"); visualization_msgs::msg::MarkerArray debug_marker_array; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); for (const auto & scene_module : scene_modules_) { - scene_module->resetVelocityFactor(); scene_module->setPlannerData(planner_data_); scene_module->modifyPathVelocity(path); // The velocity factor must be called after modifyPathVelocity. - // TODO(soblin): remove this - const auto velocity_factor = scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } for (const auto & marker : scene_module->createDebugMarkerArray().markers) { debug_marker_array.markers.push_back(marker); @@ -203,7 +184,6 @@ class SceneModuleManagerInterface virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); } - pub_velocity_factor_->publish(velocity_factor_array); planning_factor_interface_->publish(); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { @@ -274,8 +254,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::Publisher::SharedPtr pub_debug_path_; - rclcpp::Publisher::SharedPtr - pub_velocity_factor_; std::shared_ptr processing_time_publisher_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 71a5ea3b9e807..e2ddbc75c7b57 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -38,7 +38,6 @@ StopLineModule::StopLineModule( state_(State::APPROACH), debug_data_() { - velocity_factor_.init(PlanningBehavior::STOP_SIGN); } bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) @@ -62,7 +61,12 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) path->points = trajectory->restore(); - updateVelocityFactor(&velocity_factor_, state_, *stop_point - ego_s); + // TODO(soblin): PlanningFactorInterface use trajectory class + planning_factor_interface_->add( + path->points, trajectory->compute(*stop_point).point.pose, + planner_data_->current_odometry->pose, planner_data_->current_odometry->pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); updateStateAndStoppedTime( &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); @@ -152,24 +156,6 @@ void StopLineModule::updateStateAndStoppedTime( } } -void StopLineModule::updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point) -{ - switch (state) { - case State::APPROACH: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::APPROACHING); - break; - } - case State::STOPPED: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::STOPPED); - break; - } - case State::START: - break; - } -} - void StopLineModule::updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index aac830b0e9f24..7d430463c18b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -18,7 +18,6 @@ #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" -#include "autoware/motion_utils/factor/velocity_factor_interface.hpp" #include "autoware/trajectory/path_point_with_lane_id.hpp" #include @@ -98,10 +97,6 @@ class StopLineModule : public SceneModuleInterface State * state, std::optional * stopped_time, const rclcpp::Time & now, const double & distance_to_stop_point, const bool & is_vehicle_stopped) const; - static void updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point); - void updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 9a402f31af0e4..1d1ee7fc996b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -53,24 +53,13 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat autoware_perception_msgs::msg::TrafficLightGroup tl_state; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { std::shared_ptr traffic_light_scene_module( std::dynamic_pointer_cast(scene_module)); - traffic_light_scene_module->resetVelocityFactor(); traffic_light_scene_module->setPlannerData(planner_data_); traffic_light_scene_module->modifyPathVelocity(path); - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = traffic_light_scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < nearest_ref_stop_path_point_index_) { @@ -88,7 +77,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat virtual_wall_marker_creator_.add_virtual_walls( traffic_light_scene_module->createVirtualWalls()); } - pub_velocity_factor_->publish(velocity_factor_array); pub_debug_->publish(debug_marker_array); pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); pub_tl_state_->publish(tl_state); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index b051d5ff7eb76..becbaf5ee05f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -54,7 +54,6 @@ TrafficLightModule::TrafficLightModule( debug_data_(), is_prev_state_stop_(false) { - velocity_factor_.init(PlanningBehavior::TRAFFIC_SIGNAL); planner_param_ = planner_param; } @@ -293,9 +292,11 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( size_t insert_index = insert_target_point_idx; planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index); - velocity_factor_.set( + planning_factor_interface_->add( modified_path.points, planner_data_->current_odometry->pose, - target_point_with_lane_id.point.pose, VelocityFactor::UNKNOWN); + target_point_with_lane_id.point.pose, target_point_with_lane_id.point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); return modified_path; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 9ba7b0bccf1b9..a5779cfc0712a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -48,8 +48,6 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( lane_(lane), planner_param_(planner_param) { - velocity_factor_.init(PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT); - // Get map data const auto instrument = reg_elem_.getVirtualTrafficLight(); const auto instrument_bottom_line = toAutowarePoints(instrument); @@ -420,9 +418,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN, - command_.type); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_stop_line = @@ -453,8 +452,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_end_line = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 9e7a1192d9869..0661267b3d361 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -113,10 +113,6 @@ class VirtualTrafficLightModule : public SceneModuleInterface void updateInfrastructureCommand(); - void setVelocityFactor( - const geometry_msgs::msg::Pose & stop_pose, - autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor); - std::optional getPathIndexOfFirstEndLine(); bool isBeforeStartLine(const size_t end_line_idx); From 08dd641f5ab051eba69d68329cc6c1c6d752de98 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 17 Jan 2025 09:15:18 +0900 Subject: [PATCH 40/59] fix(obstacle_stop_planner): migrate planning factor (#9939) * fix(obstacle_stop_planner): migrate planning factor Signed-off-by: satoshi-ota * fix(autoware_default_adapi): add coversion map Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/debug_marker.cpp | 35 ++++++------------- .../src/debug_marker.hpp | 10 ++---- .../autoware_default_adapi/src/planning.cpp | 2 ++ 3 files changed, 16 insertions(+), 31 deletions(-) diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp index 93526518306af..62b9ae124f555 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp @@ -45,12 +45,13 @@ namespace autoware::motion_planning { ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( rclcpp::Node * node, const double base_link2front) -: node_(node), base_link2front_(base_link2front) +: node_(node), + base_link2front_(base_link2front), + planning_factor_interface_{std::make_unique( + node, "obstacle_stop_planner")} { virtual_wall_pub_ = node_->create_publisher("~/virtual_wall", 1); debug_viz_pub_ = node_->create_publisher("~/debug/marker", 1); - velocity_factor_pub_ = - node_->create_publisher("/planning/velocity_factors/obstacle_stop", 1); pub_debug_values_ = node_->create_publisher("~/obstacle_stop/debug_values", 1); } @@ -189,8 +190,13 @@ void ObstacleStopPlannerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* publish stop reason for autoware api */ - const auto velocity_factor_msg = makeVelocityFactorArray(); - velocity_factor_pub_->publish(velocity_factor_msg); + if (stop_pose_ptr_) { + planning_factor_interface_->add( + std::numeric_limits::quiet_NaN(), *stop_pose_ptr_, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + } + planning_factor_interface_->publish(); // publish debug values Float32MultiArrayStamped debug_msg{}; @@ -492,23 +498,4 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() return msg; } -VelocityFactorArray ObstacleStopPlannerDebugNode::makeVelocityFactorArray() -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = node_->now(); - - if (stop_pose_ptr_) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = PlanningBehavior::ROUTE_OBSTACLE; - velocity_factor.pose = *stop_pose_ptr_; - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - } // namespace autoware::motion_planning diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp index d13e775e3d683..dd9e5e3af1b3d 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp @@ -14,10 +14,9 @@ #ifndef DEBUG_MARKER_HPP_ #define DEBUG_MARKER_HPP_ +#include #include -#include -#include #include #include #include @@ -42,9 +41,6 @@ using std_msgs::msg::Header; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using tier4_debug_msgs::msg::Float32MultiArrayStamped; enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown, Obstacle }; @@ -109,7 +105,6 @@ class ObstacleStopPlannerDebugNode bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); MarkerArray makeVirtualWallMarker(); MarkerArray makeVisualizationMarker(); - VelocityFactorArray makeVelocityFactorArray(); void setDebugValues(const DebugValues::TYPE type, const double val) { @@ -120,11 +115,12 @@ class ObstacleStopPlannerDebugNode private: rclcpp::Publisher::SharedPtr virtual_wall_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr pub_debug_values_; rclcpp::Node * node_; double base_link2front_; + std::unique_ptr planning_factor_interface_; + std::shared_ptr stop_pose_ptr_; std::shared_ptr target_stop_pose_ptr_; std::shared_ptr slow_down_start_pose_ptr_; diff --git a/system/autoware_default_adapi/src/planning.cpp b/system/autoware_default_adapi/src/planning.cpp index e11b01115caad..98f1c846d202e 100644 --- a/system/autoware_default_adapi/src/planning.cpp +++ b/system/autoware_default_adapi/src/planning.cpp @@ -44,6 +44,7 @@ const std::map conversion_map = { {"no_stopping_area", PlanningBehavior::NO_STOPPING_AREA}, {"blind_spot", PlanningBehavior::REAR_CHECK}, {"obstacle_cruise_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"obstacle_stop_planner", PlanningBehavior::ROUTE_OBSTACLE}, {"motion_velocity_planner", PlanningBehavior::ROUTE_OBSTACLE}, {"walkway", PlanningBehavior::SIDEWALK}, {"start_planner", PlanningBehavior::START_PLANNER}, @@ -215,6 +216,7 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning "/planning/planning_factors/merge_from_private", "/planning/planning_factors/no_stopping_area", "/planning/planning_factors/obstacle_cruise_planner", + "/planning/planning_factors/obstacle_stop_planner", "/planning/planning_factors/occlusion_spot", "/planning/planning_factors/run_out", "/planning/planning_factors/stop_line", From 7bc6a4e109cfaafc44ec3cae2cf5ab44ed7190a4 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 17 Jan 2025 13:53:12 +0900 Subject: [PATCH 41/59] fix(map_based_prediction): fix unintentional accumulation of lanelets (#9950) add clear before insert Signed-off-by: a-maumau --- perception/autoware_map_based_prediction/src/predictor_vru.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/autoware_map_based_prediction/src/predictor_vru.cpp b/perception/autoware_map_based_prediction/src/predictor_vru.cpp index 0eea665cbd8a2..ffbdbd97b50a2 100644 --- a/perception/autoware_map_based_prediction/src/predictor_vru.cpp +++ b/perception/autoware_map_based_prediction/src/predictor_vru.cpp @@ -269,6 +269,7 @@ void PredictorVru::setLaneletMap(std::shared_ptr lanelet_ma const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); const auto crosswalks = lanelet::utils::query::crosswalkLanelets(all_lanelets); const auto walkways = lanelet::utils::query::walkwayLanelets(all_lanelets); + crosswalks_.clear(); crosswalks_.insert(crosswalks_.end(), crosswalks.begin(), crosswalks.end()); crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); From 74f677936177bc49c6af13032e00ee8e1b49eef5 Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Fri, 17 Jan 2025 15:55:15 +0900 Subject: [PATCH 42/59] chore(autoware_traffic_light_map_based_detector): modify docs (#9817) * modify docs Signed-off-by: MasatoSaeki * fix title Signed-off-by: MasatoSaeki * fix docs Signed-off-by: MasatoSaeki * fix word Signed-off-by: MasatoSaeki * add comment about debug markers Signed-off-by: MasatoSaeki * fix docs Signed-off-by: MasatoSaeki --------- Signed-off-by: MasatoSaeki --- .../README.md | 48 ++++++++++--------- 1 file changed, 25 insertions(+), 23 deletions(-) diff --git a/perception/autoware_traffic_light_map_based_detector/README.md b/perception/autoware_traffic_light_map_based_detector/README.md index 8a59db19ae64d..6ef54d448b415 100644 --- a/perception/autoware_traffic_light_map_based_detector/README.md +++ b/perception/autoware_traffic_light_map_based_detector/README.md @@ -1,4 +1,4 @@ -# The `autoware_traffic_light_map_based_detector` Package +# autoware_traffic_light_map_based_detector ## Overview @@ -9,34 +9,36 @@ Calibration and vibration errors can be entered as parameters, and the size of t ![traffic_light_map_based_detector_result](./docs/traffic_light_map_based_detector_result.svg) If the node receives route information, it only looks at traffic lights on that route. -If the node receives no route information, it looks at a radius of 200 meters and the angle between the traffic light and the camera is less than 40 degrees. +If the node receives no route information, it looks at them within a radius of `max_detection_range` and the angle between the traffic light and the camera is less than `car_traffic_light_max_angle_range` or `pedestrian_traffic_light_max_angle_range`. ## Input topics -| Name | Type | Description | -| -------------------- | ------------------------------------- | ----------------------- | -| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~input/camera_info` | sensor_msgs::CameraInfo | target camera parameter | -| `~input/route` | autoware_planning_msgs::LaneletRoute | optional: route | +| Name | Type | Description | +| --------------------- | ----------------------------------------- | ----------------------- | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/camera_info` | sensor_msgs::msg::CameraInfo | target camera parameter | +| `~/input/route` | autoware_planning_msgs::msg::LaneletRoute | optional: route | ## Output topics -| Name | Type | Description | -| ---------------- | ------------------------------------------- | -------------------------------------------------------------------- | -| `~output/rois` | tier4_perception_msgs::TrafficLightRoiArray | location of traffic lights in image corresponding to the camera info | -| `~expect/rois` | tier4_perception_msgs::TrafficLightRoiArray | location of traffic lights in image without any offset | -| `~debug/markers` | visualization_msgs::MarkerArray | visualization to debug | +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | ------------------------------------------------------------------------- | +| `~/output/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image corresponding to the camera info | +| `~/expect/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image without any offset | +| `~/debug/markers` | visualization_msgs::msg::MarkerArray | markers which show a line that combines from camera to each traffic light | ## Node parameters -| Parameter | Type | Description | -| ---------------------- | ------ | --------------------------------------------------------------------- | -| `max_vibration_pitch` | double | Maximum error in pitch direction. If -5~+5, it will be 10. | -| `max_vibration_yaw` | double | Maximum error in yaw direction. If -5~+5, it will be 10. | -| `max_vibration_height` | double | Maximum error in height direction. If -5~+5, it will be 10. | -| `max_vibration_width` | double | Maximum error in width direction. If -5~+5, it will be 10. | -| `max_vibration_depth` | double | Maximum error in depth direction. If -5~+5, it will be 10. | -| `max_detection_range` | double | Maximum detection range in meters. Must be positive | -| `min_timestamp_offset` | double | Minimum timestamp offset when searching for corresponding tf | -| `max_timestamp_offset` | double | Maximum timestamp offset when searching for corresponding tf | -| `timestamp_sample_len` | double | sampling length between min_timestamp_offset and max_timestamp_offset | +| Parameter | Type | Description | +| ------------------------------------------ | ------ | ----------------------------------------------------------------------------------------------- | +| `max_vibration_pitch` | double | Maximum error in pitch direction. If -5~+5, it will be 10. | +| `max_vibration_yaw` | double | Maximum error in yaw direction. If -5~+5, it will be 10. | +| `max_vibration_height` | double | Maximum error in height direction. If -5~+5, it will be 10. | +| `max_vibration_width` | double | Maximum error in width direction. If -5~+5, it will be 10. | +| `max_vibration_depth` | double | Maximum error in depth direction. If -5~+5, it will be 10. | +| `max_detection_range` | double | Maximum detection range in meters. Must be positive. | +| `min_timestamp_offset` | double | Minimum timestamp offset when searching for corresponding tf. | +| `max_timestamp_offset` | double | Maximum timestamp offset when searching for corresponding tf. | +| `timestamp_sample_len` | double | Sampling length between min_timestamp_offset and max_timestamp_offset. | +| `car_traffic_light_max_angle_range` | double | Maximum angle between the vehicular traffic light and the camera in degrees. Must be positive. | +| `pedestrian_traffic_light_max_angle_range` | double | Maximum angle between the pedestrian traffic light and the camera in degrees. Must be positive. | From 849892ec367b052b07550413269c38667204510b Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Fri, 17 Jan 2025 15:55:32 +0900 Subject: [PATCH 43/59] chore(autoware_traffic_light_fine_detector): modify docs and related params (#9818) * modify readme and related params Signed-off-by: MasatoSaeki * fix typo Signed-off-by: MasatoSaeki * fix Signed-off-by: MasatoSaeki --------- Signed-off-by: MasatoSaeki --- .../README.md | 19 ++++++++++--------- .../traffic_light_fine_detector.param.yaml | 1 + .../src/traffic_light_fine_detector_node.cpp | 2 +- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/perception/autoware_traffic_light_fine_detector/README.md b/perception/autoware_traffic_light_fine_detector/README.md index 35b55c84aa087..df9f297422fce 100644 --- a/perception/autoware_traffic_light_fine_detector/README.md +++ b/perception/autoware_traffic_light_fine_detector/README.md @@ -1,8 +1,8 @@ -# traffic_light_fine_detector +# autoware_traffic_light_fine_detector ## Purpose -It is a package for traffic light detection using YoloX-s. +It is a package for traffic light detection using YOLOX-s. ## Training Information @@ -21,17 +21,18 @@ Please visit [autoware-documentation](https://github.com/autowarefoundation/auto ## Inner-workings / Algorithms -Based on the camera image and the global ROI array detected by `map_based_detection` node, a CNN-based detection method enables highly accurate traffic light detection. +Based on the camera image and the global ROI array detected by `map_based_detector` node, a CNN-based detection method enables highly accurate traffic light detection. If can not detect traffic light, x_offset, y_offset, height and width of output ROI become `0`. +ROIs detected from YOLOX will be selected by a combination of `expect/rois`. At this time, evaluate the whole as ROIs, not just the ROI alone. ## Inputs / Outputs ### Input -| Name | Type | Description | -| --------------- | -------------------------------------------------- | ------------------------------------------------------------------- | -| `~/input/image` | `sensor_msgs/Image` | The full size camera image | -| `~/input/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector | -| `~/expect/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector without any offset | +| Name | Type | Description | +| --------------- | -------------------------------------------------- | -------------------------------------------------------------------------------------------------------------- | +| `~/input/image` | `sensor_msgs::msg::Image` | The full size camera image | +| `~/input/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector | +| `~/expect/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector without any offset, used to select the best detection results | ### Output @@ -56,7 +57,7 @@ Based on the camera image and the global ROI array detected by `map_based_detect | `data_path` | string | "$(env HOME)/autoware_data" | packages data and artifacts directory path | | `fine_detector_model_path` | string | "" | The onnx file name for yolo model | | `fine_detector_label_path` | string | "" | The label file with label names for detected objects written on it | -| `fine_detector_precision` | string | "fp32" | The inference mode: "fp32", "fp16" | +| `fine_detector_precision` | string | "fp16" | The inference mode: "fp32", "fp16" | | `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | | `gpu_id` | integer | 0 | ID for the selecting CUDA GPU device | diff --git a/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml b/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml index 36d4413472a0b..b647729a27de5 100644 --- a/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml +++ b/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml @@ -5,4 +5,5 @@ fine_detector_precision: fp16 fine_detector_score_thresh: 0.3 fine_detector_nms_thresh: 0.65 + approximate_sync: false gpu_id: 0 diff --git a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp index d9e0471a43d62..bc3d29ce456a8 100644 --- a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp +++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp @@ -61,7 +61,7 @@ TrafficLightFineDetectorNode::TrafficLightFineDetectorNode(const rclcpp::NodeOpt std::string model_path = declare_parameter("fine_detector_model_path", ""); std::string label_path = declare_parameter("fine_detector_label_path", ""); - std::string precision = declare_parameter("fine_detector_precision", "fp32"); + std::string precision = declare_parameter("fine_detector_precision", "fp16"); const uint8_t gpu_id = declare_parameter("gpu_id", 0); // Objects with a score lower than this value will be ignored. // This threshold will be ignored if specified model contains EfficientNMS_TRT module in it From bce4c44a7416f076071fa62bde37b1d21cc8e42d Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 16:33:32 +0900 Subject: [PATCH 44/59] =?UTF-8?q?feat:=20tier4=5Fdebug=5Fmsgs=20changed=20?= =?UTF-8?q?to=20autoware=5Finternal=5Fdebug=5Fmsgs=20in=20fil=E2=80=A6=20(?= =?UTF-8?q?#9853)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files control/autoware_trajectory_follower_node Signed-off-by: vish0012 --- control/autoware_trajectory_follower_node/README.md | 2 +- .../autoware/trajectory_follower_node/controller_node.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/control/autoware_trajectory_follower_node/README.md b/control/autoware_trajectory_follower_node/README.md index ac05dd67f18e6..c56154f909d1a 100644 --- a/control/autoware_trajectory_follower_node/README.md +++ b/control/autoware_trajectory_follower_node/README.md @@ -151,7 +151,7 @@ Giving the longitudinal controller information about steer convergence allows it ## Debugging -Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. +Debug information are published by the lateral and longitudinal controller using `autoware_internal_debug_msgs/Float32MultiArrayStamped` messages. A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. diff --git a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp index 1733b4bcbbb7d..c84355cc202fa 100644 --- a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp @@ -44,7 +44,7 @@ #include "tf2_msgs/msg/tf_message.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include -#include +#include #include #include @@ -63,7 +63,7 @@ namespace trajectory_follower_node using autoware::universe_utils::StopWatch; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_control_msgs::msg::ControlHorizon; -using tier4_debug_msgs::msg::Float64Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; From dc75c7ff53ce2e5e1aaafd289a19e8d1e11c559b Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 16:36:27 +0900 Subject: [PATCH 45/59] =?UTF-8?q?feat:=20tier4=5Fdebug=5Fmsgs=20changed=20?= =?UTF-8?q?to=20autoware=5Finternal=5Fdebug=5Fmsgs=20in=20fil=E2=80=A6=20(?= =?UTF-8?q?#9851)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../training_and_data_check/rosbag2.bash | 32 +++++++++---------- .../scripts/pympc_trajectory_follower.py | 6 ++-- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash index d2687b352a17d..7a50a7e118298 100644 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash @@ -7,27 +7,27 @@ gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autow gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > lateral_error.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > yaw_error.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral autoware_internal_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > lateral_error.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw autoware_internal_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > yaw_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /system/operation_mode/state autoware_adapi_v1_msgs/msg/OperationModeState --csv --qos-history keep_all --qos-reliability reliable > system_operation_mode_state.csv' gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' gnome-terminal -- bash -c 'ros2 topic echo /sensing/imu/imu_data sensor_msgs/msg/Imu --csv --qos-history keep_all --qos-reliability reliable > imu.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_y_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_y_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_v_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_v_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_yaw_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_yaw_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_acc_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_acc_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_steer_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_steer_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_X_des_converted tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_X_des_converted.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_current tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_current.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_error_prediction tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_error_prediction.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_max_trajectory_err tier4_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_max_trajectory_err.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_emergency_stop_mode tier4_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_emergency_stop_mode.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_goal_stop_mode tier4_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_goal_stop_mode.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_total_ctrl_time tier4_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_total_ctrl_time.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_calc_u_opt_time tier4_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_calc_u_opt_time.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_y_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_y_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_v_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_v_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_yaw_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_yaw_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_acc_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_acc_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_steer_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_steer_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_X_des_converted autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_X_des_converted.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_current autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_current.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_error_prediction autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_error_prediction.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_max_trajectory_err autoware_internal_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_max_trajectory_err.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_emergency_stop_mode autoware_internal_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_emergency_stop_mode.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_goal_stop_mode autoware_internal_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_goal_stop_mode.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_total_ctrl_time autoware_internal_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_total_ctrl_time.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_calc_u_opt_time autoware_internal_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_calc_u_opt_time.csv' # wait a moment to open new terminals converting rosbag2 to csv sleep 8 diff --git a/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index f767dad223f0d..104261866cf98 100755 --- a/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -21,6 +21,9 @@ from autoware_adapi_v1_msgs.msg import OperationModeState from autoware_control_msgs.msg import Control +from autoware_internal_debug_msgs.msg import BoolStamped +from autoware_internal_debug_msgs.msg import Float32MultiArrayStamped +from autoware_internal_debug_msgs.msg import Float32Stamped from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint from autoware_smart_mpc_trajectory_follower.scripts import drive_controller @@ -43,9 +46,6 @@ from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Slerp from std_msgs.msg import String -from tier4_debug_msgs.msg import BoolStamped -from tier4_debug_msgs.msg import Float32MultiArrayStamped -from tier4_debug_msgs.msg import Float32Stamped from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray From 2a6663b27472695357628bdc106494ae3ad85a4f Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 16:36:46 +0900 Subject: [PATCH 46/59] =?UTF-8?q?feat(autoware=5Fdetected=5Fobject=5Fvalid?= =?UTF-8?q?ation):=20tier4=5Fdebug=5Fmsgs=20changed=20to=20autoware=5Finte?= =?UTF-8?q?rnal=5Fdebug=5Fmsgs=20in=20fil=E2=80=A6=20(#9871)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_detected_object_validation Signed-off-by: vish0012 Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- .../src/lanelet_filter/lanelet_filter.cpp | 4 ++-- .../src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index f91c4e2036227..f1c25e8573e85 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -180,9 +180,9 @@ void ObjectLaneletFilterNode::objectCallback( std::chrono::nanoseconds( (this->get_clock()->now() - output_object_msg.header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index 979cdd3909f14..0c69b45291281 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -386,7 +386,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( std::chrono::duration( std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency); } From aa95f2a4f27f847532006e16ceee4d458a32d515 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 17 Jan 2025 17:07:04 +0900 Subject: [PATCH 47/59] chore(planning): move package directory for planning factor interface (#9948) * chore: add new package for planning factor interface Signed-off-by: satoshi-ota * chore(surround_obstacle_checker): update include file Signed-off-by: satoshi-ota * chore(obstacle_stop_planner): update include file Signed-off-by: satoshi-ota * chore(obstacle_cruise_planner): update include file Signed-off-by: satoshi-ota * chore(motion_velocity_planner): update include file Signed-off-by: satoshi-ota * chore(bpp): update include file Signed-off-by: satoshi-ota * chore(bvp-common): update include file Signed-off-by: satoshi-ota * chore(blind_spot): update include file Signed-off-by: satoshi-ota * chore(crosswalk): update include file Signed-off-by: satoshi-ota * chore(detection_area): update include file Signed-off-by: satoshi-ota * chore(intersection): update include file Signed-off-by: satoshi-ota * chore(no_drivable_area): update include file Signed-off-by: satoshi-ota * chore(no_stopping_area): update include file Signed-off-by: satoshi-ota * chore(occlusion_spot): update include file Signed-off-by: satoshi-ota * chore(run_out): update include file Signed-off-by: satoshi-ota * chore(speed_bump): update include file Signed-off-by: satoshi-ota * chore(stop_line): update include file Signed-off-by: satoshi-ota * chore(template_module): update include file Signed-off-by: satoshi-ota * chore(traffic_light): update include file Signed-off-by: satoshi-ota * chore(vtl): update include file Signed-off-by: satoshi-ota * chore(walkway): update include file Signed-off-by: satoshi-ota * chore(motion_utils): remove factor interface Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../factor/steering_factor_interface.hpp | 51 ---------------- .../factor/velocity_factor_interface.hpp | 57 ----------------- .../src/factor/steering_factor_interface.cpp | 34 ----------- .../src/factor/velocity_factor_interface.cpp | 61 ------------------- .../planner_interface.hpp | 8 ++- .../package.xml | 1 + .../package.xml | 1 + .../src/debug_marker.cpp | 5 +- .../src/debug_marker.hpp | 5 +- .../CMakeLists.txt | 11 ++++ .../README.md | 1 + .../planning_factor_interface.hpp | 10 +-- .../package.xml | 29 +++++++++ .../src}/planing_factor_interface.cpp | 6 +- .../package.xml | 1 + .../src/debug_marker.cpp | 3 +- .../src/debug_marker.hpp | 5 +- .../behavior_path_planner_node.hpp | 4 +- .../package.xml | 1 + .../interface/scene_module_interface.hpp | 4 +- .../package.xml | 1 + .../scene.hpp | 3 +- .../src/scene.cpp | 3 +- .../src/scene_crosswalk.cpp | 3 +- .../src/scene_crosswalk.hpp | 3 +- .../src/scene.cpp | 3 +- .../src/scene.hpp | 3 +- .../src/scene_intersection.cpp | 27 ++++---- .../src/scene_intersection.hpp | 3 +- .../src/scene_merge_from_private_road.cpp | 3 +- .../src/scene_merge_from_private_road.hpp | 3 +- .../src/scene.cpp | 3 +- .../src/scene.hpp | 3 +- .../src/scene_no_stopping_area.cpp | 3 +- .../src/scene_no_stopping_area.hpp | 3 +- .../src/scene_occlusion_spot.cpp | 3 +- .../src/scene_occlusion_spot.hpp | 3 +- .../scene_module_interface.hpp | 11 ++-- .../package.xml | 1 + .../src/scene_module_interface.cpp | 3 +- .../scene_module_interface_with_rtc.hpp | 3 +- .../package.xml | 1 + .../src/scene_module_interface_with_rtc.cpp | 3 +- .../src/scene.cpp | 3 +- .../src/scene.hpp | 3 +- .../src/scene.cpp | 3 +- .../src/scene.hpp | 3 +- .../src/scene.cpp | 3 +- .../src/scene.hpp | 3 +- .../test/test_scene.cpp | 2 +- .../src/scene.cpp | 3 +- .../src/scene.hpp | 3 +- .../src/scene.cpp | 3 +- .../src/scene.hpp | 3 +- .../src/scene.cpp | 3 +- .../src/scene.hpp | 3 +- .../src/scene_walkway.cpp | 3 +- .../src/scene_walkway.hpp | 3 +- .../src/dynamic_obstacle_stop_module.cpp | 5 +- .../src/out_of_lane_module.cpp | 3 +- .../plugin_module_interface.hpp | 5 +- .../package.xml | 1 + 62 files changed, 170 insertions(+), 281 deletions(-) delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/factor/steering_factor_interface.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp delete mode 100644 common/autoware_motion_utils/src/factor/steering_factor_interface.cpp delete mode 100644 common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp create mode 100644 planning/autoware_planning_factor_interface/CMakeLists.txt create mode 100644 planning/autoware_planning_factor_interface/README.md rename {common/autoware_motion_utils/include/autoware/motion_utils/factor => planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface}/planning_factor_interface.hpp (96%) create mode 100644 planning/autoware_planning_factor_interface/package.xml rename {common/autoware_motion_utils/src/factor => planning/autoware_planning_factor_interface/src}/planing_factor_interface.cpp (93%) diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/steering_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/steering_factor_interface.hpp deleted file mode 100644 index 5b701a73ac543..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/factor/steering_factor_interface.hpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__FACTOR__STEERING_FACTOR_INTERFACE_HPP_ -#define AUTOWARE__MOTION_UTILS__FACTOR__STEERING_FACTOR_INTERFACE_HPP_ - -#include -#include -#include - -#include - -namespace autoware::motion_utils -{ - -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::SteeringFactor; -using SteeringFactorBehavior = SteeringFactor::_behavior_type; -using SteeringFactorStatus = SteeringFactor::_status_type; -using geometry_msgs::msg::Pose; - -class SteeringFactorInterface -{ -public: - [[nodiscard]] SteeringFactor get() const { return steering_factor_; } - void init(const SteeringFactorBehavior & behavior) { behavior_ = behavior; } - void reset() { steering_factor_.behavior = PlanningBehavior::UNKNOWN; } - - void set( - const std::array & pose, const std::array distance, - const uint16_t direction, const uint16_t status, const std::string & detail = ""); - -private: - SteeringFactorBehavior behavior_{SteeringFactor::UNKNOWN}; - SteeringFactor steering_factor_{}; -}; - -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__FACTOR__STEERING_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp deleted file mode 100644 index 2fcde52ec7c81..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp +++ /dev/null @@ -1,57 +0,0 @@ - -// Copyright 2022-2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ -#define AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ - -#include -#include -#include -#include - -#include -#include - -namespace autoware::motion_utils -{ -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using VelocityFactorBehavior = VelocityFactor::_behavior_type; -using VelocityFactorStatus = VelocityFactor::_status_type; -using geometry_msgs::msg::Pose; - -class VelocityFactorInterface -{ -public: - [[nodiscard]] VelocityFactor get() const { return velocity_factor_; } - void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; } - void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; } - - template - void set( - const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, - const VelocityFactorStatus status, const std::string & detail = ""); - - void set( - const double & distance, const VelocityFactorStatus & status, const std::string & detail = ""); - -private: - VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; - VelocityFactor velocity_factor_{}; -}; - -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp b/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp deleted file mode 100644 index 7f12e2972ec86..0000000000000 --- a/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -namespace autoware::motion_utils -{ -void SteeringFactorInterface::set( - const std::array & pose, const std::array distance, const uint16_t direction, - const uint16_t status, const std::string & detail) -{ - steering_factor_.pose = pose; - std::array converted_distance{0.0, 0.0}; - for (int i = 0; i < 2; ++i) converted_distance[i] = static_cast(distance[i]); - steering_factor_.distance = converted_distance; - steering_factor_.behavior = behavior_; - steering_factor_.direction = direction; - steering_factor_.status = status; - steering_factor_.detail = detail; -} -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp deleted file mode 100644 index cfa3c91eac43e..0000000000000 --- a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2023-2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include -#include - -#include -#include - -namespace autoware::motion_utils -{ -template -void VelocityFactorInterface::set( - const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, - const VelocityFactorStatus status, const std::string & detail) -{ - const auto & curr_point = curr_pose.position; - const auto & stop_point = stop_pose.position; - velocity_factor_.behavior = behavior_; - velocity_factor_.pose = stop_pose; - velocity_factor_.distance = - static_cast(autoware::motion_utils::calcSignedArcLength(points, curr_point, stop_point)); - velocity_factor_.status = status; - velocity_factor_.detail = detail; -} - -void VelocityFactorInterface::set( - const double & distance, const VelocityFactorStatus & status, const std::string & detail) -{ - velocity_factor_.behavior = behavior_; - velocity_factor_.distance = static_cast(distance); - velocity_factor_.status = status; - velocity_factor_.detail = detail; -} - -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, - const VelocityFactorStatus, const std::string &); -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, - const VelocityFactorStatus, const std::string &); -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, - const VelocityFactorStatus, const std::string &); - -} // namespace autoware::motion_utils diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index 823771e173c48..f5e183afd12bd 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ -#include "autoware/motion_utils/factor/planning_factor_interface.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/obstacle_cruise_planner/common_structs.hpp" #include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/planning_factor_interface/planning_factor_interface.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" @@ -48,7 +48,8 @@ class PlannerInterface rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) - : planning_factor_interface_{std::make_unique( + : planning_factor_interface_{std::make_unique< + autoware::planning_factor_interface::PlanningFactorInterface>( &node, "obstacle_cruise_planner")}, longitudinal_info_(longitudinal_info), vehicle_info_(vehicle_info), @@ -130,7 +131,8 @@ class PlannerInterface double getSafeDistanceMargin() const { return longitudinal_info_.safe_distance_margin; } protected: - std::unique_ptr planning_factor_interface_; + std::unique_ptr + planning_factor_interface_; // Parameters bool enable_debug_info_{false}; diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 7c6d0b286c9b0..71c3b1fb1e0d5 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -25,6 +25,7 @@ autoware_objects_of_interest_marker_interface autoware_osqp_interface autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_planning_test_manager autoware_signal_processing diff --git a/planning/autoware_obstacle_stop_planner/package.xml b/planning/autoware_obstacle_stop_planner/package.xml index c9c78b3e87c3b..ba6364a2b59ee 100644 --- a/planning/autoware_obstacle_stop_planner/package.xml +++ b/planning/autoware_obstacle_stop_planner/package.xml @@ -23,6 +23,7 @@ autoware_adapi_v1_msgs autoware_motion_utils autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_planning_test_manager autoware_signal_processing diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp index 62b9ae124f555..a3337c5dd2524 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp @@ -47,8 +47,9 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( rclcpp::Node * node, const double base_link2front) : node_(node), base_link2front_(base_link2front), - planning_factor_interface_{std::make_unique( - node, "obstacle_stop_planner")} + planning_factor_interface_{ + std::make_unique( + node, "obstacle_stop_planner")} { virtual_wall_pub_ = node_->create_publisher("~/virtual_wall", 1); debug_viz_pub_ = node_->create_publisher("~/debug/marker", 1); diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp index dd9e5e3af1b3d..72e2b52c287f1 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp @@ -14,7 +14,7 @@ #ifndef DEBUG_MARKER_HPP_ #define DEBUG_MARKER_HPP_ -#include +#include #include #include @@ -119,7 +119,8 @@ class ObstacleStopPlannerDebugNode rclcpp::Node * node_; double base_link2front_; - std::unique_ptr planning_factor_interface_; + std::unique_ptr + planning_factor_interface_; std::shared_ptr stop_pose_ptr_; std::shared_ptr target_stop_pose_ptr_; diff --git a/planning/autoware_planning_factor_interface/CMakeLists.txt b/planning/autoware_planning_factor_interface/CMakeLists.txt new file mode 100644 index 0000000000000..0c25a2c21fa19 --- /dev/null +++ b/planning/autoware_planning_factor_interface/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_planning_factor_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_planning_factor_interface SHARED + DIRECTORY src +) + +ament_auto_package() diff --git a/planning/autoware_planning_factor_interface/README.md b/planning/autoware_planning_factor_interface/README.md new file mode 100644 index 0000000000000..7f715790995ba --- /dev/null +++ b/planning/autoware_planning_factor_interface/README.md @@ -0,0 +1 @@ +# planning factor interface diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp b/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp similarity index 96% rename from common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp rename to planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp index 7b720c69c6701..a2325b197853d 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp +++ b/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_ -#define AUTOWARE__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_ +#ifndef AUTOWARE__PLANNING_FACTOR_INTERFACE__PLANNING_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__PLANNING_FACTOR_INTERFACE__PLANNING_FACTOR_INTERFACE_HPP_ #include #include @@ -30,7 +30,7 @@ #include #include -namespace autoware::motion_utils +namespace autoware::planning_factor_interface { using geometry_msgs::msg::Pose; @@ -235,6 +235,6 @@ extern template void PlanningFactorInterface::add + + + autoware_planning_factor_interface + 0.40.0 + The autoware_planning_factor_interface package + Satoshi Ota + Mamoru Sobue + Apache License 2.0 + + Satoshi Ota + + ament_cmake_auto + autoware_cmake + + autoware_motion_utils + autoware_planning_msgs + autoware_universe_utils + rclcpp + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp b/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp similarity index 93% rename from common/autoware_motion_utils/src/factor/planing_factor_interface.cpp rename to planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp index d09355b4003ae..6578776dddc4f 100644 --- a/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp +++ b/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include -namespace autoware::motion_utils +namespace autoware::planning_factor_interface { template void PlanningFactorInterface::add( const std::vector &, const Pose &, const Pose &, @@ -44,4 +44,4 @@ template void PlanningFactorInterface::add &, const Pose &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, const std::string &); -} // namespace autoware::motion_utils +} // namespace autoware::planning_factor_interface diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 8bbc0b6bf4e02..d92223fb2ea5d 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -16,6 +16,7 @@ autoware_motion_utils autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index 2acf6ba1c92f5..e34315ad986a5 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -67,7 +67,8 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const double & surround_check_back_distance, const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) -: planning_factor_interface_{std::make_unique( +: planning_factor_interface_{std::make_unique< + autoware::planning_factor_interface::PlanningFactorInterface>( &node, "surround_obstacle_checker")}, vehicle_info_(vehicle_info), object_label_(object_label), diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index c49e277f2dc6c..0569c6815a252 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -15,7 +15,7 @@ #ifndef DEBUG_MARKER_HPP_ #define DEBUG_MARKER_HPP_ -#include +#include #include #include @@ -70,7 +70,8 @@ class SurroundObstacleCheckerDebugNode rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; - std::unique_ptr planning_factor_interface_; + std::unique_ptr + planning_factor_interface_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::string object_label_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index 7f7ecd7326b08..ba3d3d5d7a4d4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -20,7 +20,7 @@ #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "planner_manager.hpp" -#include +#include #include #include @@ -51,7 +51,7 @@ namespace autoware::behavior_path_planner { -using autoware::motion_utils::PlanningFactorInterface; +using autoware::planning_factor_interface::PlanningFactorInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObject; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index f9c736b9d5dde..3f41ebe74c86d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -48,6 +48,7 @@ autoware_object_recognition_utils autoware_path_sampler autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_planning_test_manager autoware_signal_processing diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index c96bc601b6403..741d4552d8558 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -21,11 +21,11 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include -#include #include #include #include #include +#include #include #include #include @@ -52,9 +52,9 @@ namespace autoware::behavior_path_planner { -using autoware::motion_utils::PlanningFactorInterface; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using autoware::planning_factor_interface::PlanningFactorInterface; using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::generateUUID; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index f23a9980f1237..22237a5a1d3a4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -52,6 +52,7 @@ autoware_object_recognition_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_route_handler autoware_rtc_interface diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 91eff0b01346a..2d7b79357b602 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -76,7 +76,8 @@ class BlindSpotModule : public SceneModuleInterfaceWithRTC const std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 4fca5f821f934..12943912f04cd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -42,7 +42,8 @@ BlindSpotModule::BlindSpotModule( const std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_{planner_param}, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 844647e33f8c0..754c14df9fac7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -177,7 +177,8 @@ CrosswalkModule::CrosswalkModule( const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), planner_param_(planner_param), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index f32ee96623822..b9803207a1a31 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -336,7 +336,8 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 7e6a320f3ddad..8641ce19e4ae5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -38,7 +38,8 @@ DetectionAreaModule::DetectionAreaModule( const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 1d3e5f3540ff3..42cc0461cd59d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -70,7 +70,8 @@ class DetectionAreaModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 5b8aca1c8ec9d..daebfbed3d12a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -51,7 +51,8 @@ IntersectionModule::IntersectionModule( const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), lane_id_(lane_id), @@ -702,7 +703,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, + [[maybe_unused]] planning_factor_interface::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); @@ -717,7 +718,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, + [[maybe_unused]] planning_factor_interface::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -731,7 +732,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, + [[maybe_unused]] planning_factor_interface::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -743,7 +744,7 @@ void reactRTCApprovalByDecisionResult( const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - motion_utils::PlanningFactorInterface * planning_factor_interface, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -790,7 +791,7 @@ void reactRTCApprovalByDecisionResult( const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - motion_utils::PlanningFactorInterface * planning_factor_interface, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -822,7 +823,7 @@ void reactRTCApprovalByDecisionResult( const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - motion_utils::PlanningFactorInterface * planning_factor_interface, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -866,7 +867,7 @@ void reactRTCApprovalByDecisionResult( const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - motion_utils::PlanningFactorInterface * planning_factor_interface, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -918,7 +919,7 @@ void reactRTCApprovalByDecisionResult( const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - motion_utils::PlanningFactorInterface * planning_factor_interface, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -976,7 +977,7 @@ void reactRTCApprovalByDecisionResult( const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - motion_utils::PlanningFactorInterface * planning_factor_interface, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -1024,7 +1025,7 @@ void reactRTCApprovalByDecisionResult( const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - motion_utils::PlanningFactorInterface * planning_factor_interface, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -1078,7 +1079,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - motion_utils::PlanningFactorInterface * planning_factor_interface, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -1121,7 +1122,7 @@ void reactRTCApprovalByDecisionResult( const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - motion_utils::PlanningFactorInterface * planning_factor_interface, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index db0cfbe98d53c..39a56977b301e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -306,7 +306,8 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); /** *********************************************************** diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index fc442337198af..d12b58a63e531 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -43,7 +43,8 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( const PlannerParam & planner_param, const std::set & associative_ids, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), associative_ids_(associative_ids) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index fe446d304d9e0..4b9c0a9c5547e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -63,7 +63,8 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface const PlannerParam & planner_param, const std::set & associative_ids, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index 0e646e92035cf..bbca1bf2d6481 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -31,7 +31,8 @@ NoDrivableLaneModule::NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_(planner_param), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index 835e946443e90..5490a37c7ec79 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -57,7 +57,8 @@ class NoDrivableLaneModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index c788a54073ed6..14f4f7b62c1f7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -43,7 +43,8 @@ NoStoppingAreaModule::NoStoppingAreaModule( const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 0d53441924805..036b6e30509c4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -59,7 +59,8 @@ class NoStoppingAreaModule : public SceneModuleInterfaceWithRTC const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index fa259b2bea9aa..935b1ec125024 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -66,7 +66,8 @@ OcclusionSpotModule::OcclusionSpotModule( const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), param_(planner_param) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 22b2a91be66b0..c744cdcb22b84 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -50,7 +50,8 @@ class OcclusionSpotModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); /** * @brief plan occlusion spot velocity at unknown area in occupancy grid diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 5bff3ca343e72..743fd0e31e387 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -17,10 +17,10 @@ #include #include -#include #include #include #include +#include #include #include #include @@ -77,7 +77,8 @@ class SceneModuleInterface explicit SceneModuleInterface( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); virtual ~SceneModuleInterface() = default; virtual bool modifyPathVelocity(PathWithLaneId * path) = 0; @@ -102,7 +103,7 @@ class SceneModuleInterface std::shared_ptr planner_data_; std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; - std::shared_ptr planning_factor_interface_; + std::shared_ptr planning_factor_interface_; void setObjectsOfInterestData( const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, @@ -136,7 +137,7 @@ class SceneModuleManagerInterface pub_virtual_wall_ = node.create_publisher( std::string("~/virtual_wall/") + module_name, 5); planning_factor_interface_ = - std::make_shared(&node, module_name); + std::make_shared(&node, module_name); processing_time_publisher_ = std::make_shared(&node, "~/debug"); @@ -261,7 +262,7 @@ class SceneModuleManagerInterface std::shared_ptr time_keeper_; - std::shared_ptr planning_factor_interface_; + std::shared_ptr planning_factor_interface_; }; extern template SceneModuleManagerInterface::SceneModuleManagerInterface( rclcpp::Node & node, [[maybe_unused]] const char * module_name); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 9f920dff8f166..bf2a9dfc5e32e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -27,6 +27,7 @@ autoware_motion_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_route_handler autoware_universe_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index db2a274272f9f..1849c05085ac4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -27,7 +27,8 @@ namespace autoware::behavior_velocity_planner SceneModuleInterface::SceneModuleInterface( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : module_id_(module_id), logger_(logger), clock_(clock), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp index a955538f4f9fe..0cf1e343fb646 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -54,7 +54,8 @@ class SceneModuleInterfaceWithRTC : public SceneModuleInterface explicit SceneModuleInterfaceWithRTC( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); virtual ~SceneModuleInterfaceWithRTC() = default; void setActivation(const bool activated) { activated_ = activated; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml index 88b252106a90a..3c2a229cca95a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml @@ -21,6 +21,7 @@ autoware_behavior_velocity_planner_common autoware_motion_utils + autoware_planning_factor_interface autoware_planning_msgs autoware_rtc_interface autoware_universe_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp index 2e73b35e20ed9..34c7a30e73acf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -29,7 +29,8 @@ namespace autoware::behavior_velocity_planner SceneModuleInterfaceWithRTC::SceneModuleInterfaceWithRTC( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), activated_(false), safe_(false), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index 193ba5eab0553..a300e8f5f9284 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -47,7 +47,8 @@ RunOutModule::RunOutModule( std::unique_ptr dynamic_obstacle_creator, const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 49de16753a869..1879d724f98b3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -49,7 +49,8 @@ class RunOutModule : public SceneModuleInterface std::unique_ptr dynamic_obstacle_creator, const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp index 4af7801e50ba3..a5099eedce027 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp @@ -36,7 +36,8 @@ SpeedBumpModule::SpeedBumpModule( const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), lane_id_(lane_id), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index e3ee2c0566381..5b908f90bf9c2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -57,7 +57,8 @@ class SpeedBumpModule : public SceneModuleInterface const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index e2ddbc75c7b57..2115a0a7eca6e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -31,7 +31,8 @@ StopLineModule::StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), stop_line_(std::move(stop_line)), planner_param_(planner_param), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 7d430463c18b9..d2c999a377ab4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -70,7 +70,8 @@ class StopLineModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp index e304b479a005d..2b176a64c597d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -76,7 +76,7 @@ class StopLineModuleTest : public ::testing::Test module_ = std::make_shared( 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_, std::make_shared(), - std::make_shared( + std::make_shared( node_.get(), "test_stopline")); module_->setPlannerData(planner_data_); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index d8eea337e7186..7dc8ca9b2e43b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -28,7 +28,8 @@ namespace autoware::behavior_velocity_planner TemplateModule::TemplateModule( const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface) { } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index ba078f3fd6421..d204e589f0416 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -31,7 +31,8 @@ class TemplateModule : public SceneModuleInterface TemplateModule( const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); /** * @brief Modify the velocity of path points. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index becbaf5ee05f2..3a8692e9c53dd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -45,7 +45,8 @@ TrafficLightModule::TrafficLightModule( lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index c30cbbdfc1477..71f0855a4af6f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -71,7 +71,8 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index a5779cfc0712a..a42e3520ea3c1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -41,7 +41,8 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), reg_elem_(reg_elem), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 0661267b3d361..24e77cfaea157 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -84,7 +84,8 @@ class VirtualTrafficLightModule : public SceneModuleInterface const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 94421ae27e077..91ed11a7467d5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -35,7 +35,8 @@ WalkwayModule::WalkwayModule( const PlannerParam & planner_param, const bool use_regulatory_element, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface) + const std::shared_ptr + planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), state_(State::APPROACH), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index 1bd5003498d34..3e06bf2d878c6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -46,7 +46,8 @@ class WalkwayModule : public SceneModuleInterface const PlannerParam & planner_param, const bool use_regulatory_element, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, - const std::shared_ptr planning_factor_interface); + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 2000615b51e2c..c7a103e0269ca 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -44,8 +44,9 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo logger_ = node.get_logger().get_child(ns_); clock_ = node.get_clock(); - planning_factor_interface_ = std::make_unique( - &node, "dynamic_obstacle_stop"); + planning_factor_interface_ = + std::make_unique( + &node, "dynamic_obstacle_stop"); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 532cc1e8c7d51..ab69218ea9668 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -60,7 +60,8 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) init_parameters(node); planning_factor_interface_ = - std::make_unique(&node, "out_of_lane"); + std::make_unique( + &node, "out_of_lane"); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index 6d683acff667f..c650e324de16a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -18,7 +18,7 @@ #include "planner_data.hpp" #include "velocity_planning_result.hpp" -#include +#include #include #include @@ -55,7 +55,8 @@ class PluginModuleInterface autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; protected: - std::unique_ptr planning_factor_interface_; + std::unique_ptr + planning_factor_interface_; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index 07b719689f51f..5f4c15e0f8ab9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -23,6 +23,7 @@ autoware_motion_utils autoware_motion_velocity_planner_common autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_universe_utils autoware_velocity_smoother From 62fa8ce18643ba44caaf5016fc4f4ff1e8365cd7 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 17:16:01 +0900 Subject: [PATCH 48/59] feat(autoware_planning_validator)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_planning_validator (#9911) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_planning_validator Signed-off-by: vish0012 --- .../autoware/planning_validator/planning_validator.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp index e996855b9b4da..298e7f74c289b 100644 --- a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp +++ b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp @@ -26,10 +26,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -37,13 +37,13 @@ namespace autoware::planning_validator { using autoware::universe_utils::StopWatch; +using autoware_internal_debug_msgs::msg::Float64Stamped; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_planning_validator::msg::PlanningValidatorStatus; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float64Stamped; struct ValidationParams { From c926e46aaefbb37c1738cd7b20269a8e4dd602b8 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 17:16:57 +0900 Subject: [PATCH 49/59] =?UTF-8?q?feat:=20tier4=5Fdebug=5Fmsgs=20changed=20?= =?UTF-8?q?to=20autoware=5Finternal=5Fdebug=5Fmsgs=20in=20fil=E2=80=A6=20(?= =?UTF-8?q?#9904)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_mission_planner Signed-off-by: vish0012 --- .../src/mission_planner/mission_planner.cpp | 6 +++--- .../src/mission_planner/mission_planner.hpp | 5 +++-- .../src/mission_planner/route_selector.cpp | 6 +++--- .../src/mission_planner/route_selector.hpp | 5 +++-- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp index cbbcfb84d2cbb..b806feded9802 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -87,14 +87,14 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) is_mission_planner_ready_ = false; logger_configure_ = std::make_unique(this); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); } void MissionPlanner::publish_processing_time( autoware::universe_utils::StopWatch stop_watch) { - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp index 987ca6482ec11..18f0d3a9c259d 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -28,9 +28,9 @@ #include #include #include +#include #include #include -#include #include #include #include @@ -146,7 +146,8 @@ class MissionPlanner : public rclcpp::Node bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); std::unique_ptr logger_configure_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; }; } // namespace autoware::mission_planner diff --git a/planning/autoware_mission_planner/src/mission_planner/route_selector.cpp b/planning/autoware_mission_planner/src/mission_planner/route_selector.cpp index 5ee565af00423..ad7daba18dece 100644 --- a/planning/autoware_mission_planner/src/mission_planner/route_selector.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/route_selector.cpp @@ -138,14 +138,14 @@ RouteSelector::RouteSelector(const rclcpp::NodeOptions & options) main_request_ = std::monostate{}; // Processing time - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); } void RouteSelector::publish_processing_time( autoware::universe_utils::StopWatch stop_watch) { - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); diff --git a/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp b/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp index 5c8f81aaf216e..3e52f6962c47e 100644 --- a/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp @@ -18,8 +18,8 @@ #include #include +#include #include -#include #include #include #include @@ -82,7 +82,8 @@ class RouteSelector : public rclcpp::Node rclcpp::Client::SharedPtr cli_set_lanelet_route_; rclcpp::Subscription::SharedPtr sub_state_; rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; bool initialized_; bool mrm_operating_; From b466796bbe9464e191f31e1e3a885f7025fad11f Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 17:17:26 +0900 Subject: [PATCH 50/59] =?UTF-8?q?feat:=20tier4=5Fdebug=5Fmsgs=20changed=20?= =?UTF-8?q?to=20autoware=5Finternal=5Fdebug=5Fmsgs=20in=20fil=E2=80=A6=20(?= =?UTF-8?q?#9902)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_external_velocity_limit_selector Signed-off-by: vish0012 --- .../external_velocity_limit_selector_node.hpp | 4 ++-- .../autoware_external_velocity_limit_selector/package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index 6a8fae3bf394c..be4893d5a49c6 100644 --- a/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -29,7 +29,7 @@ namespace autoware::external_velocity_limit_selector { -using tier4_debug_msgs::msg::StringStamped; +using autoware_internal_debug_msgs::msg::StringStamped; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using tier4_planning_msgs::msg::VelocityLimitConstraints; diff --git a/planning/autoware_external_velocity_limit_selector/package.xml b/planning/autoware_external_velocity_limit_selector/package.xml index 5bfd3fa936d50..4a4fac40bd8a5 100644 --- a/planning/autoware_external_velocity_limit_selector/package.xml +++ b/planning/autoware_external_velocity_limit_selector/package.xml @@ -17,10 +17,10 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs generate_parameter_library rclcpp rclcpp_components - tier4_debug_msgs tier4_planning_msgs ros2cli From f9c0aa69cba891f88d7b9afdf5e9d7cd89c0bd17 Mon Sep 17 00:00:00 2001 From: interimadd Date: Fri, 17 Jan 2025 17:35:29 +0900 Subject: [PATCH 51/59] fix(imu_corrector): remove non-periodic publish to /diagnostics topic (#9951) fix(imu_corrector): remove force_update() in timer callback Signed-off-by: Takahisa.Ishikawa Co-authored-by: Takahisa.Ishikawa --- sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp index 8dcc024ffc2b4..17194ac1b0459 100644 --- a/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp @@ -40,7 +40,6 @@ GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) { updater_.setHardwareID(get_name()); updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); - // diagnostic_updater is designed to be updated at the same rate as the timer updater_.setPeriod(diagnostics_updater_interval_sec_); gyro_bias_estimation_module_ = std::make_unique(); @@ -182,8 +181,6 @@ void GyroBiasEstimator::timer_callback() transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); validate_gyro_bias(); - updater_.force_update(); - updater_.setPeriod(diagnostics_updater_interval_sec_); // to reset timer inside the updater } void GyroBiasEstimator::validate_gyro_bias() From 18da07da01719ad948b5131c79e730aa36a7bae2 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 21:23:09 +0900 Subject: [PATCH 52/59] feat(autoware_processing_time_checker)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_processing_time_checker (#9921) Signed-off-by: vish0012 Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- system/autoware_processing_time_checker/README.md | 6 +++--- system/autoware_processing_time_checker/package.xml | 2 +- .../src/processing_time_checker.hpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/system/autoware_processing_time_checker/README.md b/system/autoware_processing_time_checker/README.md index 1cb6a289bc54f..e7d70f9378f10 100644 --- a/system/autoware_processing_time_checker/README.md +++ b/system/autoware_processing_time_checker/README.md @@ -17,9 +17,9 @@ ros2 launch autoware_processing_time_checker processing_time_checker.launch.xml ### Input -| Name | Type | Description | -| ------------------------- | --------------------------------- | ------------------------------ | -| `/.../processing_time_ms` | `tier4_debug_msgs/Float64Stamped` | processing time of each module | +| Name | Type | Description | +| ------------------------- | --------------------------------------------- | ------------------------------ | +| `/.../processing_time_ms` | `autoware_internal_debug_msgs/Float64Stamped` | processing time of each module | ### Output diff --git a/system/autoware_processing_time_checker/package.xml b/system/autoware_processing_time_checker/package.xml index 4a18857663cc1..37f53c94de0d1 100644 --- a/system/autoware_processing_time_checker/package.xml +++ b/system/autoware_processing_time_checker/package.xml @@ -12,11 +12,11 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs autoware_universe_utils nlohmann-json-dev rclcpp rclcpp_components - tier4_debug_msgs tier4_metric_msgs ament_lint_auto diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.hpp b/system/autoware_processing_time_checker/src/processing_time_checker.hpp index 77450923509f2..f6664b901f890 100644 --- a/system/autoware_processing_time_checker/src/processing_time_checker.hpp +++ b/system/autoware_processing_time_checker/src/processing_time_checker.hpp @@ -19,7 +19,7 @@ #include -#include +#include #include #include @@ -32,7 +32,7 @@ namespace autoware::processing_time_checker using autoware::universe_utils::Accumulator; using MetricMsg = tier4_metric_msgs::msg::Metric; using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; -using tier4_debug_msgs::msg::Float64Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; class ProcessingTimeChecker : public rclcpp::Node { From b92eb33c95b91fad1883c337b215f0dc002645eb Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 21:25:28 +0900 Subject: [PATCH 53/59] feat(autoware_steer_offset_estimator)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_steer_offset_estimator (#9926) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files vehicle/autoware_steer_offset_estimator Signed-off-by: vish0012 Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- vehicle/autoware_steer_offset_estimator/Readme.md | 8 ++++---- .../steer_offset_estimator_node.hpp | 4 ++-- vehicle/autoware_steer_offset_estimator/package.xml | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/vehicle/autoware_steer_offset_estimator/Readme.md b/vehicle/autoware_steer_offset_estimator/Readme.md index 2cd6bd2aabe2a..a7e9b6722a648 100644 --- a/vehicle/autoware_steer_offset_estimator/Readme.md +++ b/vehicle/autoware_steer_offset_estimator/Readme.md @@ -24,10 +24,10 @@ Calculate yaw rate error and then calculate steering error recursively by least ### Output -| Name | Type | Description | -| ------------------------------------- | --------------------------------------- | ----------------------------- | -| `~/output/steering_offset` | `tier4_debug_msgs::msg::Float32Stamped` | steering offset | -| `~/output/steering_offset_covariance` | `tier4_debug_msgs::msg::Float32Stamped` | covariance of steering offset | +| Name | Type | Description | +| ------------------------------------- | --------------------------------------------------- | ----------------------------- | +| `~/output/steering_offset` | `autoware_internal_debug_msgs::msg::Float32Stamped` | steering offset | +| `~/output/steering_offset_covariance` | `autoware_internal_debug_msgs::msg::Float32Stamped` | covariance of steering offset | ## Launch Calibrator diff --git a/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp b/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp index 4e03cbe0162fe..0d7896d53a705 100644 --- a/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp +++ b/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp @@ -18,16 +18,16 @@ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" #include namespace autoware::steer_offset_estimator { +using autoware_internal_debug_msgs::msg::Float32Stamped; using geometry_msgs::msg::TwistStamped; -using tier4_debug_msgs::msg::Float32Stamped; using Steering = autoware_vehicle_msgs::msg::SteeringReport; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; diff --git a/vehicle/autoware_steer_offset_estimator/package.xml b/vehicle/autoware_steer_offset_estimator/package.xml index 715d80504dafe..e098fbdee6f28 100644 --- a/vehicle/autoware_steer_offset_estimator/package.xml +++ b/vehicle/autoware_steer_offset_estimator/package.xml @@ -9,6 +9,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -17,7 +18,6 @@ rclcpp rclcpp_components std_msgs - tier4_debug_msgs autoware_global_parameter_loader autoware_pose2twist From e3e7fdb72b21f490e19152b97e12fe3cfcd4a1f0 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 21:30:41 +0900 Subject: [PATCH 54/59] feat(autoware_accel_brake_map_calibrator)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_accel_brake_map_calibrator (#9923) Signed-off-by: vish0012 Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- .../accel_brake_map_calibrator_node.hpp | 8 ++++---- vehicle/autoware_accel_brake_map_calibrator/package.xml | 2 +- .../scripts/accel_tester.py | 2 +- .../scripts/actuation_cmd_publisher.py | 2 +- .../scripts/brake_tester.py | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 2a0ad4f987dad..cd05bed5ce476 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -28,6 +28,8 @@ #include +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -37,8 +39,6 @@ #include "std_msgs/msg/multi_array_dimension.hpp" #include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_external_api_msgs/msg/calibration_status.hpp" #include "tier4_external_api_msgs/msg/calibration_status_array.hpp" #include "tier4_external_api_msgs/srv/get_accel_brake_map_calibration_data.hpp" @@ -57,6 +57,8 @@ namespace autoware::accel_brake_map_calibrator { +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_vehicle_msgs::msg::SteeringReport; using autoware_vehicle_msgs::msg::VelocityReport; using geometry_msgs::msg::TwistStamped; @@ -64,8 +66,6 @@ using nav_msgs::msg::OccupancyGrid; using raw_vehicle_cmd_converter::AccelMap; using raw_vehicle_cmd_converter::BrakeMap; using std_msgs::msg::Float32MultiArray; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Float32Stamped; using tier4_external_api_msgs::msg::CalibrationStatus; using tier4_vehicle_msgs::msg::ActuationCommandStamped; using tier4_vehicle_msgs::msg::ActuationStatusStamped; diff --git a/vehicle/autoware_accel_brake_map_calibrator/package.xml b/vehicle/autoware_accel_brake_map_calibrator/package.xml index de2dcf2d8e440..f166bd119ed6a 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/package.xml +++ b/vehicle/autoware_accel_brake_map_calibrator/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_motion_utils autoware_raw_vehicle_cmd_converter autoware_universe_utils @@ -25,7 +26,6 @@ std_srvs tf2_geometry_msgs tf2_ros - tier4_debug_msgs tier4_external_api_msgs tier4_vehicle_msgs visualization_msgs diff --git a/vehicle/autoware_accel_brake_map_calibrator/scripts/accel_tester.py b/vehicle/autoware_accel_brake_map_calibrator/scripts/accel_tester.py index f73718f7ea7bc..8aef30f9ee251 100755 --- a/vehicle/autoware_accel_brake_map_calibrator/scripts/accel_tester.py +++ b/vehicle/autoware_accel_brake_map_calibrator/scripts/accel_tester.py @@ -15,9 +15,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +from autoware_internal_debug_msgs.msg import Float32Stamped import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped MAX_ACCEL = 1.0 # [-] MIN_ACCEL = 0.0 # [-] diff --git a/vehicle/autoware_accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py b/vehicle/autoware_accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py index 3a029e193f41f..334b5652dc8b6 100755 --- a/vehicle/autoware_accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py +++ b/vehicle/autoware_accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py @@ -15,10 +15,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +from autoware_internal_debug_msgs.msg import Float32Stamped from autoware_vehicle_msgs.msg import GearCommand import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped from tier4_vehicle_msgs.msg import ActuationCommandStamped diff --git a/vehicle/autoware_accel_brake_map_calibrator/scripts/brake_tester.py b/vehicle/autoware_accel_brake_map_calibrator/scripts/brake_tester.py index 050f232334afd..fe067494a0c39 100755 --- a/vehicle/autoware_accel_brake_map_calibrator/scripts/brake_tester.py +++ b/vehicle/autoware_accel_brake_map_calibrator/scripts/brake_tester.py @@ -16,9 +16,9 @@ # limitations under the License. +from autoware_internal_debug_msgs.msg import Float32Stamped import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped MAX_BRAKE = 1.0 # [-] MIN_BRAKE = 0.0 # [-] From fc763acd1f1cdcda1358976a963da5f367e61bb6 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 21:32:20 +0900 Subject: [PATCH 55/59] feat(autoware_raw_vehicle_cmd_converter)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_raw_vehicle_cmd_converter (#9924) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files vehicle/autoware_raw_vehicle_cmd_converter Signed-off-by: vish0012 Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- .../include/autoware_raw_vehicle_cmd_converter/node.hpp | 4 ++-- vehicle/autoware_raw_vehicle_cmd_converter/package.xml | 2 +- vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp index 8f8e543dc680e..eaf39cf729971 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp @@ -28,10 +28,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -42,7 +42,7 @@ namespace autoware::raw_vehicle_cmd_converter { using Control = autoware_control_msgs::msg::Control; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using tier4_vehicle_msgs::msg::ActuationCommandStamped; using tier4_vehicle_msgs::msg::ActuationStatusStamped; using TwistStamped = geometry_msgs::msg::TwistStamped; diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml index ff00f09a7efd8..0abb77644920a 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml @@ -23,13 +23,13 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_vehicle_msgs geometry_msgs nav_msgs rclcpp rclcpp_components - tier4_debug_msgs tier4_vehicle_msgs rclpy diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp index 7624705b15b37..8d05e4faa69ad 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp @@ -245,7 +245,7 @@ double RawVehicleCommandConverterNode::calculateSteerFromMap( debug_steer_.setValues(DebugValues::TYPE::ERROR_P, pid_errors.at(0)); debug_steer_.setValues(DebugValues::TYPE::ERROR_I, pid_errors.at(1)); debug_steer_.setValues(DebugValues::TYPE::ERROR_D, pid_errors.at(2)); - tier4_debug_msgs::msg::Float32MultiArrayStamped msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped msg{}; for (const auto & v : debug_steer_.getValues()) { msg.data.push_back(v); } From 64e4e21512ec6538532cac3d2417fd94d2467416 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Fri, 17 Jan 2025 21:48:00 +0900 Subject: [PATCH 56/59] =?UTF-8?q?feat:=20tier4=5Fdebug=5Fmsgs=20changed=20?= =?UTF-8?q?to=20autoware=5Finternal=5Fdebug=5Fmsgs=20in=20fil=E2=80=A6=20(?= =?UTF-8?q?#9896)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_raindrop_cluster_filter Signed-off-by: vish0012 --- .../src/low_intensity_cluster_filter_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp index e52fc57a5a20c..70ddeadf00b8e 100644 --- a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp +++ b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp @@ -114,9 +114,9 @@ void LowIntensityClusterFilter::objectCallback( if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); } } From 4b166614d1dc6e372498f52e2fba14f4f79a03fa Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 19 Jan 2025 17:40:26 +0900 Subject: [PATCH 57/59] feat(autoware_planning_test_manager): remove dependency of VirtualTrafficLightState and ExpandStopRange (#9953) * feat(autoware_planning_test_manager): remove dependency of virtual traffic light Signed-off-by: Takayuki Murooka * modify obstacle_stop test code Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- ...e_obstacle_stop_planner_node_interface.cpp | 18 +++++++++++++-- .../autoware_planning_test_manager.hpp | 13 +---------- .../package.xml | 1 - .../src/autoware_planning_test_manager.cpp | 20 ++++------------ .../test/test_node_interface.cpp | 23 +++++++++++++++---- 5 files changed, 41 insertions(+), 34 deletions(-) diff --git a/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp b/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp index 9f840586a1959..9da8409c6c2ae 100644 --- a/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp +++ b/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp @@ -18,6 +18,8 @@ #include #include +#include + #include #include @@ -25,6 +27,7 @@ using autoware::motion_planning::ObstacleStopPlannerNode; using autoware::planning_test_manager::PlanningInterfaceTestManager; +using tier4_planning_msgs::msg::ExpandStopRange; std::shared_ptr generateTestManager() { @@ -66,8 +69,17 @@ void publishMandatoryTopics( test_manager->publishPointCloud(test_target_node, "obstacle_stop_planner/input/pointcloud"); test_manager->publishAcceleration(test_target_node, "obstacle_stop_planner/input/acceleration"); test_manager->publishPredictedObjects(test_target_node, "obstacle_stop_planner/input/objects"); - test_manager->publishExpandStopRange( - test_target_node, "obstacle_stop_planner/input/expand_stop_range"); +} + +void publishExpandStopRange( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + auto test_node = test_manager->getTestNode(); + const auto expand_stop_range = test_manager->getTestNode()->create_publisher( + "obstacle_stop_planner/input/expand_stop_range", 1); + expand_stop_range->publish(ExpandStopRange{}); + autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -78,6 +90,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); + publishExpandStopRange(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); @@ -97,6 +110,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); + publishExpandStopRange(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp index 2c9a935c2077b..aa7836bc66b88 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp @@ -52,14 +52,10 @@ #include #include #include -#include -#include -#include #include #include #include #include -#include #include #include @@ -87,14 +83,10 @@ using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; -using tier4_api_msgs::msg::CrosswalkStatus; -using tier4_api_msgs::msg::IntersectionStatus; -using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::LateralOffset; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::VelocityLimit; -using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; enum class ModuleName { UNKNOWN = 0, @@ -117,7 +109,6 @@ class PlanningInterfaceTestManager void publishPointCloud(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishAcceleration(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishPredictedObjects(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishExpandStopRange(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishOccupancyGrid(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishCostMap(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishMap(rclcpp::Node::SharedPtr target_node, std::string topic_name); @@ -131,7 +122,6 @@ class PlanningInterfaceTestManager void publishLateralOffset(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishOperationModeState(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishVirtualTrafficLightState(rclcpp::Node::SharedPtr target_node, std::string topic_name); void setTrajectoryInputTopicName(std::string topic_name); void setParkingTrajectoryInputTopicName(std::string topic_name); @@ -178,6 +168,7 @@ class PlanningInterfaceTestManager void testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node); int getReceivedTopicNum(); + rclcpp::Node::SharedPtr getTestNode() const; private: // Publisher (necessary for node running) @@ -187,7 +178,6 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr point_cloud_pub_; rclcpp::Publisher::SharedPtr acceleration_pub_; rclcpp::Publisher::SharedPtr predicted_objects_pub_; - rclcpp::Publisher::SharedPtr expand_stop_range_pub_; rclcpp::Publisher::SharedPtr occupancy_grid_pub_; rclcpp::Publisher::SharedPtr cost_map_pub_; rclcpp::Publisher::SharedPtr map_pub_; @@ -202,7 +192,6 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr lateral_offset_pub_; rclcpp::Publisher::SharedPtr operation_mode_state_pub_; rclcpp::Publisher::SharedPtr traffic_signals_pub_; - rclcpp::Publisher::SharedPtr virtual_traffic_light_states_pub_; // Subscriber rclcpp::Subscription::SharedPtr traj_sub_; diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index 80801a8102453..d3395a6c518f3 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -31,7 +31,6 @@ rclcpp tf2_msgs tf2_ros - tier4_api_msgs tier4_planning_msgs tier4_v2x_msgs unique_identifier_msgs diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 62f89cab44e7b..9bc73f3455821 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -71,13 +71,6 @@ void PlanningInterfaceTestManager::publishPredictedObjects( test_node_, target_node, topic_name, predicted_objects_pub_, PredictedObjects{}); } -void PlanningInterfaceTestManager::publishExpandStopRange( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, expand_stop_range_pub_, ExpandStopRange{}); -} - void PlanningInterfaceTestManager::publishOccupancyGrid( rclcpp::Node::SharedPtr target_node, std::string topic_name) { @@ -181,14 +174,6 @@ void PlanningInterfaceTestManager::publishTrafficSignals( test_node_, target_node, topic_name, traffic_signals_pub_, TrafficLightGroupArray{}); } -void PlanningInterfaceTestManager::publishVirtualTrafficLightState( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, virtual_traffic_light_states_pub_, - VirtualTrafficLightStateArray{}); -} - void PlanningInterfaceTestManager::publishInitialPoseTF( rclcpp::Node::SharedPtr target_node, std::string topic_name) { @@ -471,4 +456,9 @@ int PlanningInterfaceTestManager::getReceivedTopicNum() return count_; } +rclcpp::Node::SharedPtr PlanningInterfaceTestManager::getTestNode() const +{ + return test_node_; +} + } // namespace autoware::planning_test_manager diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp index ab2fd5847c5be..0e22413c0c8ec 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp @@ -13,6 +13,9 @@ // limitations under the License. #include +#include + +#include #include @@ -23,6 +26,19 @@ namespace autoware::behavior_velocity_planner { +using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; + +void publishVirtualTrafficLightState( + std::shared_ptr test_manager, rclcpp::Node::SharedPtr target_node) +{ + auto test_node = test_manager->getTestNode(); + const auto pub_virtual_traffic_light = + test_manager->getTestNode()->create_publisher( + "behavior_velocity_planner_node/input/virtual_traffic_light_states", 1); + pub_virtual_traffic_light->publish(VirtualTrafficLightStateArray{}); + autoware::test_utils::spinSomeNodes(test_node, target_node, 3); +} + TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) { rclcpp::init(0, nullptr); @@ -35,8 +51,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); - test_manager->publishVirtualTrafficLightState( - test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); + publishVirtualTrafficLightState(test_manager, test_target_node); // test with nominal path_with_lane_id ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); @@ -57,9 +72,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); - test_manager->publishVirtualTrafficLightState( - test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); + publishVirtualTrafficLightState(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); From 37fc242731cdc0e3e805cc377665eee60eb23b26 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 19 Jan 2025 21:47:27 +0900 Subject: [PATCH 58/59] refactor(behavior_path_planner): common test functions (#9963) * feat: common test code in behavior_path_planner Signed-off-by: Takayuki Murooka * deal with other modules Signed-off-by: Takayuki Murooka * fix typo Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- ...t_behavior_path_planner_node_interface.cpp | 90 ++-------------- ...t_behavior_path_planner_node_interface.cpp | 82 ++------------ ...t_behavior_path_planner_node_interface.cpp | 90 +++------------- ...t_behavior_path_planner_node_interface.cpp | 83 ++------------ .../CMakeLists.txt | 1 + .../behavior_path_planner/test_utils.hpp | 42 ++++++++ .../src/test_utils.cpp | 101 ++++++++++++++++++ ...e_behavior_path_planner_node_interface.cpp | 73 ++----------- ...t_behavior_path_planner_node_interface.cpp | 80 ++------------ ...t_behavior_path_planner_node_interface.cpp | 85 ++------------- 10 files changed, 214 insertions(+), 513 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/test_utils.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index f679efacb6a44..349d92377d091 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,11 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" - -#include -#include -#include +#include "autoware/behavior_path_planner/test_utils.hpp" #include @@ -25,84 +21,18 @@ #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); - - // set behavior_path_planner's input topic name(this topic is changed to test node) - test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); - - test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); - const auto behavior_path_lane_change_module_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_lane_change_module"); - - std::vector module_names; - module_names.emplace_back("autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", - ament_index_cpp::get_package_share_directory( - "autoware_behavior_path_static_obstacle_avoidance_module") + - "/config/static_obstacle_avoidance.param.yaml", - ament_index_cpp::get_package_share_directory( - "autoware_behavior_path_avoidance_by_lane_change_module") + - "/config/avoidance_by_lane_change.param.yaml"}); - - return std::make_shared(node_options); -} - -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - // publish necessary topics from test_manager - test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); - test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_path_planner/input/occupancy_grid_map"); - test_manager->publishLaneDrivingScenario( - test_target_node, "behavior_path_planner/input/scenario"); - test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); - test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); - test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); - test_manager->publishLateralOffset( - test_target_node, "behavior_path_planner/input/lateral_offset"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode( + {"lane_change", "static_obstacle_avoidance", "avoidance_by_lane_change"}, + {"autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory @@ -119,7 +49,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"lane_change", "static_obstacle_avoidance", "avoidance_by_lane_change"}, + {"autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index a18a2440dcb3a..488475079613f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,87 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" - -#include -#include -#include +#include "autoware/behavior_path_planner/test_utils.hpp" #include #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); - - // set behavior_path_planner's input topic name(this topic is changed to test node) - test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); - - test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); - - std::vector module_names; - module_names.emplace_back("autoware::behavior_path_planner::DynamicAvoidanceModuleManager"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory( - "autoware_behavior_path_dynamic_obstacle_avoidance_module") + - "/config/dynamic_obstacle_avoidance.param.yaml"}); - - return std::make_shared(node_options); -} - -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - // publish necessary topics from test_manager - test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); - test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_path_planner/input/occupancy_grid_map"); - test_manager->publishLaneDrivingScenario( - test_target_node, "behavior_path_planner/input/scenario"); - test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); - test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); - test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); - test_manager->publishLateralOffset( - test_target_node, "behavior_path_planner/input/lateral_offset"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"dynamic_obstacle_avoidance"}, + {"autoware::behavior_path_planner::DynamicObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); @@ -110,7 +46,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"dynamic_obstacle_avoidance"}, + {"autoware::behavior_path_planner::DynamicObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index ad37a90605233..506e43a94de1d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" -#include "autoware_planning_test_manager/autoware_planning_test_manager.hpp" -#include "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware/behavior_path_planner/test_utils.hpp" #include @@ -24,83 +21,19 @@ #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); - - // set behavior_path_planner's input topic name(this topic is changed to test node) - test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); - - test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); - const auto behavior_path_lane_change_module_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_lane_change_module"); - - std::vector module_names; - module_names.emplace_back( - "autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); - module_names.emplace_back( - "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, { - autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", - }); - - return std::make_shared(node_options); -} - -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - // publish necessary topics from test_manager - test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); - test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_path_planner/input/occupancy_grid_map"); - test_manager->publishLaneDrivingScenario( - test_target_node, "behavior_path_planner/input/scenario"); - test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); - test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); - test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); - test_manager->publishLateralOffset( - test_target_node, "behavior_path_planner/input/lateral_offset"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode( + {"lane_change"}, + {"autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager", + "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory @@ -117,7 +50,10 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"lane_change"}, + {"autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager", + "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 555657fe34af9..f6b3e9eeaf715 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" -#include "autoware_planning_test_manager/autoware_planning_test_manager.hpp" -#include "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware/behavior_path_planner/test_utils.hpp" #include @@ -24,78 +21,18 @@ #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); - - // set behavior_path_planner's input topic name(this topic is changed to test node) - test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); - - test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); - const auto behavior_path_lane_change_module_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_lane_change_module"); - - std::vector module_names; - module_names.emplace_back("autoware::behavior_path_planner::LaneChangeRightModuleManager"); - module_names.emplace_back("autoware::behavior_path_planner::LaneChangeLeftModuleManager"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml"}); - - return std::make_shared(node_options); -} - -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - // publish necessary topics from test_manager - test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); - test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_path_planner/input/occupancy_grid_map"); - test_manager->publishLaneDrivingScenario( - test_target_node, "behavior_path_planner/input/scenario"); - test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); - test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); - test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); - test_manager->publishLateralOffset( - test_target_node, "behavior_path_planner/input/lateral_offset"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"lane_change"}, {"autoware::behavior_path_planner::LaneChangeRightModuleManager", + "autoware::behavior_path_planner::LaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); @@ -113,7 +50,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"lane_change"}, {"autoware::behavior_path_planner::LaneChangeRightModuleManager", + "autoware::behavior_path_planner::LaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner/CMakeLists.txt index cadaacd01d9c9..f43b454cfb100 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(magic_enum CONFIG REQUIRED) ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/planner_manager.cpp src/behavior_path_planner_node.cpp + src/test_utils.cpp ) target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/test_utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/test_utils.hpp new file mode 100644 index 0000000000000..5e1718419f139 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/test_utils.hpp @@ -0,0 +1,42 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_PLANNER__TEST_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER__TEST_UTILS_HPP_ + +#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::BehaviorPathPlannerNode; +using autoware::planning_test_manager::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager(); + +std::shared_ptr generateNode( + const std::vector & module_name_vec, + const std::vector & plugin_name_vec); + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node); +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER__TEST_UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp new file mode 100644 index 0000000000000..2ce6f5bee9a50 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp @@ -0,0 +1,101 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_planner/test_utils.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode( + const std::vector & module_name_vec, + const std::vector & plugin_name_vec) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); + + std::vector plugin_names; + for (const auto & plugin_name : plugin_name_vec) { + plugin_names.emplace_back(plugin_name); + } + + std::vector params; + params.emplace_back("launch_modules", plugin_names); + node_options.parameter_overrides(params); + + const auto get_behavior_path_module_config = [](const std::string & module) { + const auto package_name = "autoware_behavior_path_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + + auto yaml_files = std::vector{ + autoware_test_utils_dir + "/config/test_common.param.yaml", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml"}; + for (const auto & module_name : module_name_vec) { + yaml_files.push_back(get_behavior_path_module_config(module_name)); + } + + autoware::test_utils::updateNodeOptions(node_options, yaml_files); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp index 63b7ccf3fff12..0c933345e7647 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp @@ -13,10 +13,7 @@ // limitations under the License. #include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" - -#include -#include -#include +#include "autoware/behavior_path_planner/test_utils.hpp" #include @@ -25,73 +22,15 @@ #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); - - // set behavior_path_planner's input topic name(this topic is changed to test node) - test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); - - test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); - - std::vector module_names; - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml"}); - - return std::make_shared(node_options); -} - -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - // publish necessary topics from test_manager - test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); - test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_path_planner/input/occupancy_grid_map"); - test_manager->publishLaneDrivingScenario( - test_target_node, "behavior_path_planner/input/scenario"); - test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); - test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); - test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); - test_manager->publishLateralOffset( - test_target_node, "behavior_path_planner/input/lateral_offset"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode({}, {}); publishMandatoryTopics(test_manager, test_target_node); @@ -109,7 +48,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode({}, {}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index c8661cdf1d912..c248a40eb151a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,11 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" - -#include -#include -#include +#include "autoware/behavior_path_planner/test_utils.hpp" #include @@ -25,77 +21,16 @@ #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); - - // set behavior_path_planner's input topic name(this topic is changed to test node) - test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); - - test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); - - std::vector module_names; - module_names.emplace_back("autoware::behavior_path_planner::SideShiftModuleManager"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, - {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("autoware_behavior_path_side_shift_module") + - "/config/side_shift.param.yaml"}); - - return std::make_shared(node_options); -} - -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - // publish necessary topics from test_manager - test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); - test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_path_planner/input/occupancy_grid_map"); - test_manager->publishLaneDrivingScenario( - test_target_node, "behavior_path_planner/input/scenario"); - test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); - test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); - test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); - test_manager->publishLateralOffset( - test_target_node, "behavior_path_planner/input/lateral_offset"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = + generateNode({"side_shift"}, {"autoware::behavior_path_planner::SideShiftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); @@ -113,7 +48,8 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = + generateNode({"side_shift"}, {"autoware::behavior_path_planner::SideShiftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index b6a8fd04f93db..c2531e5f2bb9f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,89 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" - -#include -#include -#include +#include "autoware/behavior_path_planner/test_utils.hpp" #include #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: behavior_path_planner → test_node_ - test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); - - // set behavior_path_planner's input topic name(this topic is changed to test node) - test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); - - test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); - - std::vector module_names; - module_names.emplace_back( - "autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory( - "autoware_behavior_path_static_obstacle_avoidance_module") + - "/config/static_obstacle_avoidance.param.yaml"}); - - return std::make_shared(node_options); -} - -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - // publish necessary topics from test_manager - test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); - test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_path_planner/input/occupancy_grid_map"); - test_manager->publishLaneDrivingScenario( - test_target_node, "behavior_path_planner/input/scenario"); - test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); - test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); - test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); - test_manager->publishLateralOffset( - test_target_node, "behavior_path_planner/input/lateral_offset"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode( + {"static_obstacle_avoidance"}, + {"autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory @@ -111,7 +46,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"static_obstacle_avoidance"}, + {"autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory From 80eb07ccf960a290e8d96ebfa0f351e1fbb2a032 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 19 Jan 2025 23:47:56 +0900 Subject: [PATCH 59/59] feat(autoware_planning_test_manager): remove dependency of tier4_planning_msgs::msg::LateralOffset (#9967) * feat(autoware_planning_test_manager): remove dependency of tier4_planning_msgs::msg::LateralOffset Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- common/autoware_test_utils/package.xml | 2 -- .../autoware_planning_test_manager.hpp | 4 ---- .../package.xml | 1 - .../src/autoware_planning_test_manager.cpp | 7 ------- .../src/test_utils.cpp | 21 +++++++++++++++++-- 5 files changed, 19 insertions(+), 16 deletions(-) diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml index 740e7f840141d..f67b366480b00 100644 --- a/common/autoware_test_utils/package.xml +++ b/common/autoware_test_utils/package.xml @@ -33,9 +33,7 @@ std_srvs tf2_msgs tf2_ros - tier4_api_msgs tier4_planning_msgs - tier4_v2x_msgs unique_identifier_msgs yaml_cpp_vendor diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp index aa7836bc66b88..bb808484e10be 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp @@ -52,7 +52,6 @@ #include #include #include -#include #include #include #include @@ -83,7 +82,6 @@ using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; -using tier4_planning_msgs::msg::LateralOffset; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::VelocityLimit; @@ -119,7 +117,6 @@ class PlanningInterfaceTestManager void publishRoute(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishTF(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishInitialPoseTF(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishLateralOffset(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishOperationModeState(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name); @@ -189,7 +186,6 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr route_pub_; rclcpp::Publisher::SharedPtr TF_pub_; rclcpp::Publisher::SharedPtr initial_pose_tf_pub_; - rclcpp::Publisher::SharedPtr lateral_offset_pub_; rclcpp::Publisher::SharedPtr operation_mode_state_pub_; rclcpp::Publisher::SharedPtr traffic_signals_pub_; diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index d3395a6c518f3..d5f8383913274 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -32,7 +32,6 @@ tf2_msgs tf2_ros tier4_planning_msgs - tier4_v2x_msgs unique_identifier_msgs yaml_cpp_vendor diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 9bc73f3455821..f07efd0cda116 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -153,13 +153,6 @@ void PlanningInterfaceTestManager::publishTF( autoware::test_utils::makeTFMsg(target_node, "base_link", "map")); } -void PlanningInterfaceTestManager::publishLateralOffset( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, lateral_offset_pub_, LateralOffset{}); -} - void PlanningInterfaceTestManager::publishOperationModeState( rclcpp::Node::SharedPtr target_node, std::string topic_name) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp index 2ce6f5bee9a50..4930ead6d355d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp @@ -18,12 +18,30 @@ #include #include +#include + #include #include #include namespace autoware::behavior_path_planner { +namespace +{ +using tier4_planning_msgs::msg::LateralOffset; + +void publishLateralOffset( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + auto test_node = test_manager->getTestNode(); + const auto pub_lateral_offset = test_manager->getTestNode()->create_publisher( + "behavior_path_planner/input/lateral_offset", 1); + pub_lateral_offset->publish(LateralOffset{}); + autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3); +} +} // namespace + std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); @@ -95,7 +113,6 @@ void publishMandatoryTopics( test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); - test_manager->publishLateralOffset( - test_target_node, "behavior_path_planner/input/lateral_offset"); + publishLateralOffset(test_manager, test_target_node); } } // namespace autoware::behavior_path_planner