diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index 23508e63bc43d..93533b54e1dd0 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -54,7 +54,7 @@ jobs: - name: Select verb id: select-verb run: | - has_previous_draft=$(gh release view --json isDraft -q ".isDraft" "${{ steps.set-tag-name.outputs.tag-name }}") + has_previous_draft=$(gh release view --json isDraft -q ".isDraft" "${{ steps.set-tag-name.outputs.tag-name }}") || true verb=create if [ "$has_previous_draft" = "true" ]; then diff --git a/common/autoware_auto_common/CMakeLists.txt b/common/autoware_auto_common/CMakeLists.txt index 4f7846c89eb72..b0957ad726856 100644 --- a/common/autoware_auto_common/CMakeLists.txt +++ b/common/autoware_auto_common/CMakeLists.txt @@ -9,7 +9,7 @@ include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) if(BUILD_TESTING) set(TEST_COMMON test_common_gtest) - ament_add_gtest(${TEST_COMMON} + ament_add_ros_isolated_gtest(${TEST_COMMON} test/gtest_main.cpp test/test_bool_comparisons.cpp test/test_byte_reader.cpp diff --git a/common/autoware_auto_common/package.xml b/common/autoware_auto_common/package.xml index dca22e9b29ea3..17d666e2273e6 100644 --- a/common/autoware_auto_common/package.xml +++ b/common/autoware_auto_common/package.xml @@ -14,7 +14,7 @@ builtin_interfaces eigen - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common geometry_msgs diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt index 0e964e3b064a2..454e0e7ef044f 100644 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ b/common/autoware_auto_geometry/CMakeLists.txt @@ -23,7 +23,7 @@ if(BUILD_TESTING) test/src/test_common_2d.cpp test/src/test_intersection.cpp ) - ament_add_gtest(${GEOMETRY_GTEST} ${GEOMETRY_SRC}) + ament_add_ros_isolated_gtest(${GEOMETRY_GTEST} ${GEOMETRY_SRC}) target_compile_options(${GEOMETRY_GTEST} PRIVATE -Wno-conversion -Wno-sign-conversion) target_include_directories(${GEOMETRY_GTEST} PRIVATE "test/include" "include") ament_target_dependencies(${GEOMETRY_GTEST} diff --git a/common/autoware_auto_geometry/package.xml b/common/autoware_auto_geometry/package.xml index 976cf59013370..e19ac4897aab0 100644 --- a/common/autoware_auto_geometry/package.xml +++ b/common/autoware_auto_geometry/package.xml @@ -18,7 +18,7 @@ autoware_auto_vehicle_msgs geometry_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common osrf_testing_tools_cpp diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt index f4b3223391a7b..a8ae9ec2d962e 100755 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ b/common/autoware_auto_tf2/CMakeLists.txt @@ -5,7 +5,7 @@ find_package(autoware_cmake REQUIRED) autoware_package() if(BUILD_TESTING) - ament_add_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) + ament_add_ros_isolated_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) target_include_directories(test_tf2_autoware_auto_msgs PRIVATE "include") ament_target_dependencies(test_tf2_autoware_auto_msgs "autoware_auto_common" diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml index d6fbd4f4ce3d8..c80ce45a217ac 100644 --- a/common/autoware_auto_tf2/package.xml +++ b/common/autoware_auto_tf2/package.xml @@ -20,7 +20,7 @@ tf2_geometry_msgs tf2_ros - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/autoware_point_types/CMakeLists.txt b/common/autoware_point_types/CMakeLists.txt index e46081c5ea96f..c149f79dab71f 100644 --- a/common/autoware_point_types/CMakeLists.txt +++ b/common/autoware_point_types/CMakeLists.txt @@ -11,7 +11,7 @@ include_directories( ) if(BUILD_TESTING) - ament_add_gtest(test_autoware_point_types + ament_add_ros_isolated_gtest(test_autoware_point_types test/test_point_types.cpp ) target_include_directories(test_autoware_point_types diff --git a/common/autoware_point_types/package.xml b/common/autoware_point_types/package.xml index b7449dbbd13d5..8829bd7538d9b 100644 --- a/common/autoware_point_types/package.xml +++ b/common/autoware_point_types/package.xml @@ -23,7 +23,7 @@ pcl_ros point_cloud_msg_wrapper - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common point_cloud_msg_wrapper diff --git a/common/autoware_testing/autoware_testing/smoke_test.py b/common/autoware_testing/autoware_testing/smoke_test.py index 2753861269124..8c361474034ad 100644 --- a/common/autoware_testing/autoware_testing/smoke_test.py +++ b/common/autoware_testing/autoware_testing/smoke_test.py @@ -16,6 +16,7 @@ import os import shlex +import time import unittest from ament_index_python import get_package_share_directory @@ -26,21 +27,24 @@ from launch_ros.actions import Node import launch_testing import pytest +import rclpy def resolve_node(context, *args, **kwargs): + parameters = [ + os.path.join( + get_package_share_directory(LaunchConfiguration("arg_package").perform(context)), + "param", + file_name, + ) + for file_name in shlex.split(LaunchConfiguration("arg_param_filenames").perform(context)) + ] smoke_test_node = Node( package=LaunchConfiguration("arg_package"), executable=LaunchConfiguration("arg_package_exe"), namespace="test", - parameters=[ - os.path.join( - get_package_share_directory(LaunchConfiguration("arg_package").perform(context)), - "param", - LaunchConfiguration("arg_param_filename").perform(context), - ) - ], + parameters=parameters, arguments=shlex.split(LaunchConfiguration("arg_executable_arguments").perform(context)), ) return [smoke_test_node] @@ -55,8 +59,8 @@ def generate_test_description(): arg_package_exe = DeclareLaunchArgument( "arg_package_exe", default_value=["default"], description="Tested executable" ) - arg_param_filename = DeclareLaunchArgument( - "arg_param_filename", default_value=["test.param.yaml"], description="Test param file" + arg_param_filenames = DeclareLaunchArgument( + "arg_param_filenames", default_value=["test.param.yaml"], description="Test param file" ) arg_executable_arguments = DeclareLaunchArgument( "arg_executable_arguments", default_value=[""], description="Tested executable arguments" @@ -66,7 +70,7 @@ def generate_test_description(): [ arg_package, arg_package_exe, - arg_param_filename, + arg_param_filenames, arg_executable_arguments, OpaqueFunction(function=resolve_node), launch_testing.actions.ReadyToTest(), @@ -74,8 +78,19 @@ def generate_test_description(): ) +class DummyTest(unittest.TestCase): + def test_wait_for_node_ready(self): + """Waiting for the node is ready.""" + rclpy.init() + test_node = rclpy.create_node("test_node") + while len(test_node.get_node_names()) == 0: + time.sleep(0.1) + print("waiting for Node to be ready") + rclpy.shutdown() + + @launch_testing.post_shutdown_test() class TestProcessOutput(unittest.TestCase): def test_exit_code(self, proc_output, proc_info): - # Check that process exits with code -15 code: termination request, sent to the program - launch_testing.asserts.assertExitCodes(proc_info, [-15]) + # Check that process exits with code 0 + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/common/autoware_testing/cmake/add_smoke_test.cmake b/common/autoware_testing/cmake/add_smoke_test.cmake index 2998db4960de4..ee2cb0f06e789 100644 --- a/common/autoware_testing/cmake/add_smoke_test.cmake +++ b/common/autoware_testing/cmake/add_smoke_test.cmake @@ -19,18 +19,18 @@ # :type package_name: string # :param package_exec: package executable to run during smoke test # :type executable_name: string -# :param PARAM_FILENAME: yaml filename containing test parameters -# :type PARAM_FILENAME: string +# :param PARAM_FILENAMES: yaml filenames containing test parameters +# :type PARAM_FILENAMES: string # :param EXECUTABLE_ARGUMENTS: arguments passed to tested executable # :type EXECUTABLE_ARGUMENTS: string function(add_smoke_test package_name executable_name) - cmake_parse_arguments(PARSE_ARGV 2 smoke_test "" "PARAM_FILENAME;EXECUTABLE_ARGUMENTS" "") + cmake_parse_arguments(PARSE_ARGV 2 smoke_test "" "PARAM_FILENAMES;EXECUTABLE_ARGUMENTS" "") set(ARGUMENTS "arg_package:=${package_name}" "arg_package_exe:=${executable_name}") - if(smoke_test_PARAM_FILENAME) - list(APPEND ARGUMENTS "arg_param_filename:=${smoke_test_PARAM_FILENAME}") + if(smoke_test_PARAM_FILENAMES) + list(APPEND ARGUMENTS "arg_param_filenames:=${smoke_test_PARAM_FILENAMES}") endif() if(smoke_test_EXECUTABLE_ARGUMENTS) diff --git a/common/fake_test_node/CMakeLists.txt b/common/fake_test_node/CMakeLists.txt index cc80077163ab1..8ad71df2777f3 100644 --- a/common/fake_test_node/CMakeLists.txt +++ b/common/fake_test_node/CMakeLists.txt @@ -7,7 +7,7 @@ autoware_package() ament_auto_add_library(fake_test_node SHARED src/fake_test_node.cpp) if(BUILD_TESTING) - ament_add_gtest(test_fake_test_node + ament_add_ros_isolated_gtest(test_fake_test_node test/test_fake_test_node.cpp ) add_dependencies(test_fake_test_node fake_test_node) diff --git a/common/fake_test_node/package.xml b/common/fake_test_node/package.xml index 6d337906ff3b7..8e919d719fc6c 100644 --- a/common/fake_test_node/package.xml +++ b/common/fake_test_node/package.xml @@ -11,7 +11,7 @@ autoware_cmake - ament_cmake_gtest + ament_cmake_ros autoware_auto_common rclcpp rclcpp_components diff --git a/common/had_map_utils/package.xml b/common/had_map_utils/package.xml index 2baa56aa12c73..aa7f7a9e78840 100644 --- a/common/had_map_utils/package.xml +++ b/common/had_map_utils/package.xml @@ -23,7 +23,7 @@ lanelet2_io visualization_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/interpolation/CMakeLists.txt b/common/interpolation/CMakeLists.txt index bca79e0f23d98..7afed6fe57c41 100644 --- a/common/interpolation/CMakeLists.txt +++ b/common/interpolation/CMakeLists.txt @@ -13,7 +13,7 @@ ament_auto_add_library(interpolation SHARED if(BUILD_TESTING) file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_gtest(test_interpolation ${test_files}) + ament_add_ros_isolated_gtest(test_interpolation ${test_files}) target_link_libraries(test_interpolation interpolation diff --git a/common/interpolation/package.xml b/common/interpolation/package.xml index 213616b007a19..665e26bab9a0a 100644 --- a/common/interpolation/package.xml +++ b/common/interpolation/package.xml @@ -14,7 +14,7 @@ tier4_autoware_utils - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/motion_testing/CMakeLists.txt b/common/motion_testing/CMakeLists.txt index 4e7006978ab22..f8428c0df709c 100644 --- a/common/motion_testing/CMakeLists.txt +++ b/common/motion_testing/CMakeLists.txt @@ -7,7 +7,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/motion_testing/motion_testing.cpp) if(BUILD_TESTING) - ament_add_gtest(motion_testing_unit_tests + ament_add_ros_isolated_gtest(motion_testing_unit_tests test/gtest_main.cpp test/constant_trajectory.cpp test/trajectory_checks.cpp) diff --git a/common/motion_testing/package.xml b/common/motion_testing/package.xml index 33e7b4798d0e7..f292a7e5bb54b 100644 --- a/common/motion_testing/package.xml +++ b/common/motion_testing/package.xml @@ -19,7 +19,7 @@ autoware_auto_planning_msgs autoware_auto_vehicle_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/osqp_interface/CMakeLists.txt b/common/osqp_interface/CMakeLists.txt index 4180461cc261c..996f8b6153cf0 100644 --- a/common/osqp_interface/CMakeLists.txt +++ b/common/osqp_interface/CMakeLists.txt @@ -49,7 +49,7 @@ if(BUILD_TESTING) test/test_csc_matrix_conv.cpp ) set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) - ament_add_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) + ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) endif() diff --git a/common/osqp_interface/package.xml b/common/osqp_interface/package.xml index f89810ddda953..5260a3f2b07c0 100644 --- a/common/osqp_interface/package.xml +++ b/common/osqp_interface/package.xml @@ -17,7 +17,7 @@ rclcpp rclcpp_components - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common eigen diff --git a/common/signal_processing/CMakeLists.txt b/common/signal_processing/CMakeLists.txt index e93023e703eb7..93cb7aa225a08 100644 --- a/common/signal_processing/CMakeLists.txt +++ b/common/signal_processing/CMakeLists.txt @@ -9,7 +9,7 @@ ament_auto_add_library(lowpass_filter_1d SHARED ) if(BUILD_TESTING) - ament_add_gtest(test_signal_processing + ament_add_ros_isolated_gtest(test_signal_processing test/src/lowpass_filter_1d_test.cpp ) target_link_libraries(test_signal_processing diff --git a/common/signal_processing/package.xml b/common/signal_processing/package.xml index 563715af77248..867f2d05da60e 100644 --- a/common/signal_processing/package.xml +++ b/common/signal_processing/package.xml @@ -12,7 +12,7 @@ ament_cmake_auto - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/tier4_api_utils/CMakeLists.txt b/common/tier4_api_utils/CMakeLists.txt index e65e538fe1df4..4eda0296e520e 100644 --- a/common/tier4_api_utils/CMakeLists.txt +++ b/common/tier4_api_utils/CMakeLists.txt @@ -6,7 +6,7 @@ autoware_package() if(BUILD_TESTING) include_directories(include) - ament_add_gtest(${PROJECT_NAME}_test test/test.cpp) + ament_add_ros_isolated_gtest(${PROJECT_NAME}_test test/test.cpp) ament_target_dependencies(${PROJECT_NAME}_test rclcpp tier4_external_api_msgs) endif() diff --git a/common/tier4_api_utils/package.xml b/common/tier4_api_utils/package.xml index 99a03ab92b0ec..a8378b92352ec 100644 --- a/common/tier4_api_utils/package.xml +++ b/common/tier4_api_utils/package.xml @@ -14,7 +14,7 @@ rclcpp tier4_external_api_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common rclcpp diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index eeca0a2d83630..7ba9c2020a36f 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -12,11 +12,11 @@ ament_auto_add_library(tier4_autoware_utils SHARED ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_ros REQUIRED) file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_gtest(test_tier4_autoware_utils ${test_files}) + ament_add_ros_isolated_gtest(test_tier4_autoware_utils ${test_files}) target_link_libraries(test_tier4_autoware_utils tier4_autoware_utils diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index f1bd523a8eda1..63fd56ae09f86 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -23,7 +23,7 @@ tier4_debug_msgs visualization_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 2afbe9c315f75..74f7a7418815e 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -21,6 +21,7 @@ rviz_common tier4_control_msgs tier4_external_api_msgs + tier4_planning_msgs ament_lint_auto autoware_lint_common diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 78681106b3d61..825e06dac1cd9 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -80,13 +80,26 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa engage_button_ptr_ = new QPushButton("Engage"); connect(engage_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareEngage())); + velocity_limit_button_ptr_ = new QPushButton("Send Velocity Limit"); + pub_velocity_limit_input_ = new QSpinBox(); + pub_velocity_limit_input_->setRange(-100.0, 100.0); + pub_velocity_limit_input_->setValue(0.0); + pub_velocity_limit_input_->setSingleStep(5.0); + connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit())); + auto * v_layout = new QVBoxLayout; + auto * velocity_limit_layout = new QHBoxLayout(); v_layout->addLayout(gate_layout); v_layout->addLayout(selector_layout); v_layout->addLayout(state_layout); v_layout->addLayout(gear_layout); v_layout->addLayout(engage_status_layout); v_layout->addWidget(engage_button_ptr_); + v_layout->addLayout(engage_status_layout); + velocity_limit_layout->addWidget(velocity_limit_button_ptr_); + velocity_limit_layout->addWidget(pub_velocity_limit_input_); + velocity_limit_layout->addWidget(new QLabel(" [km/h]")); + v_layout->addLayout(velocity_limit_layout); setLayout(v_layout); } @@ -114,6 +127,9 @@ void AutowareStatePanel::onInitialize() client_engage_ = raw_node_->create_client( "/api/autoware/set/engage", rmw_qos_profile_services_default); + + pub_velocity_limit_ = raw_node_->create_publisher( + "/planning/scenario_planning/max_velocity_default", rclcpp::QoS(1)); } void AutowareStatePanel::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg) @@ -215,6 +231,13 @@ void AutowareStatePanel::onEngageStatus( engage_status_label_ptr_->setText(QString::fromStdString(Bool2String(current_engage_))); } +void AutowareStatePanel::onClickVelocityLimit() +{ + auto velocity_limit = std::make_shared(); + velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; + pub_velocity_limit_->publish(*velocity_limit); +} + void AutowareStatePanel::onClickAutowareEngage() { auto req = std::make_shared(); diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index 45cd290e5f4a3..97c559bdfb3de 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include namespace rviz_plugins { @@ -41,6 +43,7 @@ class AutowareStatePanel : public rviz_common::Panel public Q_SLOTS: void onClickAutowareEngage(); + void onClickVelocityLimit(); protected: void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg); @@ -61,12 +64,16 @@ public Q_SLOTS: rclcpp::Client::SharedPtr client_engage_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + QLabel * gate_mode_label_ptr_; QLabel * selector_mode_label_ptr_; QLabel * autoware_state_label_ptr_; QLabel * gear_label_ptr_; QLabel * engage_status_label_ptr_; QPushButton * engage_button_ptr_; + QPushButton * velocity_limit_button_ptr_; + QSpinBox * pub_velocity_limit_input_; bool current_engage_; }; diff --git a/common/time_utils/package.xml b/common/time_utils/package.xml index 9b553289e09be..a248e72f125f3 100644 --- a/common/time_utils/package.xml +++ b/common/time_utils/package.xml @@ -12,7 +12,7 @@ autoware_cmake builtin_interfaces - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/vehicle_constants_manager/CMakeLists.txt b/common/vehicle_constants_manager/CMakeLists.txt index eb5f7b27663e3..1e84c5f781d39 100644 --- a/common/vehicle_constants_manager/CMakeLists.txt +++ b/common/vehicle_constants_manager/CMakeLists.txt @@ -18,7 +18,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED # if(BUILD_TESTING) # set(TEST_SOURCES test/test_vehicle_constants_manager.cpp) # set(TEST_VEHICLE_CONSTANTS_MANAGER_EXE test_vehicle_constants_manager) -# ament_add_gtest(${TEST_VEHICLE_CONSTANTS_MANAGER_EXE} ${TEST_SOURCES}) +# ament_add_ros_isolated_gtest(${TEST_VEHICLE_CONSTANTS_MANAGER_EXE} ${TEST_SOURCES}) # target_link_libraries(${TEST_VEHICLE_CONSTANTS_MANAGER_EXE} ${PROJECT_NAME}) # endif() diff --git a/common/vehicle_constants_manager/package.xml b/common/vehicle_constants_manager/package.xml index eecb3e8f2108f..8041ae90a2cb7 100644 --- a/common/vehicle_constants_manager/package.xml +++ b/common/vehicle_constants_manager/package.xml @@ -15,7 +15,7 @@ rclcpp rclcpp_components - ament_cmake_gtest + ament_cmake_ros ament_cmake diff --git a/control/trajectory_follower/CMakeLists.txt b/control/trajectory_follower/CMakeLists.txt index 5c29617296780..5d564ba33c62b 100644 --- a/control/trajectory_follower/CMakeLists.txt +++ b/control/trajectory_follower/CMakeLists.txt @@ -7,6 +7,7 @@ autoware_package() # lateral_controller set(LATERAL_CONTROLLER_LIB lateral_controller_lib) set(LATERAL_CONTROLLER_LIB_SRC + src/mpc_lateral_controller.cpp src/interpolate.cpp src/lowpass_filter.cpp src/mpc.cpp @@ -21,6 +22,9 @@ set(LATERAL_CONTROLLER_LIB_SRC ) set(LATERAL_CONTROLLER_LIB_HEADERS + include/trajectory_follower/lateral_controller_base.hpp + include/trajectory_follower/mpc_lateral_controller.hpp + include/trajectory_follower/sync_data.hpp include/trajectory_follower/visibility_control.hpp include/trajectory_follower/interpolate.hpp include/trajectory_follower/lowpass_filter.hpp @@ -45,12 +49,16 @@ target_compile_options(${LATERAL_CONTROLLER_LIB} PRIVATE -Wno-error=old-style-ca # longitudinal_controller set(LONGITUDINAL_CONTROLLER_LIB longitudinal_controller_lib) set(LONGITUDINAL_CONTROLLER_LIB_SRC + src/pid_longitudinal_controller.cpp src/pid.cpp src/smooth_stop.cpp src/longitudinal_controller_utils.cpp ) set(LONGITUDINAL_CONTROLLER_LIB_HEADERS + include/trajectory_follower/longitudinal_controller_base.hpp + include/trajectory_follower/pid_longitudinal_controller.hpp + include/trajectory_follower/sync_data.hpp include/trajectory_follower/debug_values.hpp include/trajectory_follower/pid.hpp include/trajectory_follower/smooth_stop.hpp @@ -72,7 +80,7 @@ if(BUILD_TESTING) test/test_lowpass_filter.cpp ) set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller) - ament_add_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) + ament_add_ros_isolated_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${LATERAL_CONTROLLER_LIB}) set(TEST_LON_SOURCES @@ -82,7 +90,7 @@ if(BUILD_TESTING) test/test_longitudinal_controller_utils.cpp ) set(TEST_LONGITUDINAL_CONTROLLER_EXE test_longitudinal_controller) - ament_add_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) + ament_add_ros_isolated_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${LONGITUDINAL_CONTROLLER_LIB}) endif() diff --git a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp new file mode 100644 index 0000000000000..44c26c26a0820 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp @@ -0,0 +1,55 @@ +// Copyright 2022 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 TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ +#define TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "trajectory_follower/sync_data.hpp" + +#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +struct LateralOutput +{ + autoware_auto_control_msgs::msg::AckermannLateralCommand control_cmd; + LateralSyncData sync_data; +}; + +class LateralControllerBase +{ +public: + virtual LateralOutput run() = 0; + void sync(LongitudinalSyncData longitudinal_sync_data) + { + longitudinal_sync_data_ = longitudinal_sync_data; + }; + +protected: + LongitudinalSyncData longitudinal_sync_data_; +}; + +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp new file mode 100644 index 0000000000000..cb50b1a6af031 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp @@ -0,0 +1,51 @@ +// Copyright 2022 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 TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#define TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "trajectory_follower/sync_data.hpp" + +#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +struct LongitudinalOutput +{ + autoware_auto_control_msgs::msg::LongitudinalCommand control_cmd; + LongitudinalSyncData sync_data; +}; +class LongitudinalControllerBase +{ +public: + virtual LongitudinalOutput run() = 0; + void sync(LateralSyncData lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; }; + +protected: + LateralSyncData lateral_sync_data_; +}; + +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp new file mode 100644 index 0000000000000..db05acaab33f6 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -0,0 +1,238 @@ +// 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 TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ +#define TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ + +#include "common/types.hpp" +#include "osqp_interface/osqp_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "trajectory_follower/interpolate.hpp" +#include "trajectory_follower/lateral_controller_base.hpp" +#include "trajectory_follower/lowpass_filter.hpp" +#include "trajectory_follower/mpc.hpp" +#include "trajectory_follower/mpc_trajectory.hpp" +#include "trajectory_follower/mpc_utils.hpp" +#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" +#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" +#include "trajectory_follower/sync_data.hpp" +#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "trajectory_follower/visibility_control.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::bool8_t; +using autoware::common::types::float64_t; +namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; + +class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralControllerBase +{ +public: + /** + * @brief constructor + */ + explicit MpcLateralController(rclcpp::Node * node); + + /** + * @brief destructor + */ + virtual ~MpcLateralController(); + +private: + rclcpp::Node::SharedPtr node_; + + //!< @brief topic publisher for control command + rclcpp::Publisher::SharedPtr + m_pub_ctrl_cmd; + //!< @brief topic publisher for predicted trajectory + rclcpp::Publisher::SharedPtr m_pub_predicted_traj; + //!< @brief topic publisher for control diagnostic + rclcpp::Publisher::SharedPtr + m_pub_diagnostic; + //!< @brief topic subscription for reference waypoints + rclcpp::Subscription::SharedPtr m_sub_ref_path; + //!< @brief subscription for current velocity + rclcpp::Subscription::SharedPtr m_sub_odometry; + //!< @brief subscription for current steering + rclcpp::Subscription::SharedPtr m_sub_steering; + //!< @brief timer to update after a given interval + rclcpp::TimerBase::SharedPtr m_timer; + //!< @brief subscription for transform messages + rclcpp::Subscription::SharedPtr m_tf_sub; + rclcpp::Subscription::SharedPtr m_tf_static_sub; + + /* parameters for path smoothing */ + //!< @brief flag for path smoothing + bool8_t m_enable_path_smoothing; + //!< @brief param of moving average filter for path smoothing + int64_t m_path_filter_moving_ave_num; + //!< @brief point-to-point index distance for curvature calculation for trajectory //NOLINT + int64_t m_curvature_smoothing_num_traj; + //!< @brief point-to-point index distance for curvature calculation for reference steer command + //!< //NOLINT + int64_t m_curvature_smoothing_num_ref_steer; + //!< @brief path resampling interval [m] + float64_t m_traj_resample_dist; + + /* parameters for stop state */ + float64_t m_stop_state_entry_ego_speed; + float64_t m_stop_state_entry_target_speed; + float64_t m_converged_steer_rad; + bool m_check_steer_converged_for_stopped_state; + + // MPC object + trajectory_follower::MPC m_mpc; + + //!< @brief measured pose + geometry_msgs::msg::PoseStamped::SharedPtr m_current_pose_ptr; + //!< @brief measured velocity + nav_msgs::msg::Odometry::SharedPtr m_current_odometry_ptr; + //!< @brief measured steering + autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr m_current_steering_ptr; + //!< @brief reference trajectory + autoware_auto_planning_msgs::msg::Trajectory::SharedPtr m_current_trajectory_ptr; + + //!< @brief mpc filtered output in previous period + float64_t m_steer_cmd_prev = 0.0; + + //!< @brief flag of m_ctrl_cmd_prev initialization + bool8_t m_is_ctrl_cmd_prev_initialized = false; + //!< @brief previous control command + autoware_auto_control_msgs::msg::AckermannLateralCommand m_ctrl_cmd_prev; + + //!< @brief buffer for transforms + tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME}; + tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; + + LateralSyncData lateral_sync_data_; + + //!< initialize timer to work in real, simulation, and replay + void initTimer(float64_t period_s); + /** + * @brief compute and publish control command for path follow with a constant control period + */ + LateralOutput run() override; + + /** + * @brief set m_current_trajectory with received message + */ + void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); + + /** + * @brief update current_pose from tf + * @return true if the current pose was updated, false otherwise + */ + bool8_t updateCurrentPose(); + + /** + * @brief check if the received data is valid. + */ + bool8_t checkData() const; + + /** + * @brief set current_velocity with received message + */ + void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); + + /** + * @brief set current_steering with received message + */ + void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); + + /** + * @brief create control command + * @param [in] cmd published control command + */ + autoware_auto_control_msgs::msg::AckermannLateralCommand createCtrlCmdMsg( + autoware_auto_control_msgs::msg::AckermannLateralCommand cmd); + + /** + * @brief publish predicted future trajectory + * @param [in] predicted_traj published predicted trajectory + */ + void publishPredictedTraj(autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const; + + /** + * @brief publish diagnostic message + * @param [in] diagnostic published diagnostic + */ + void publishDiagnostic( + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) const; + + /** + * @brief get stop command + */ + autoware_auto_control_msgs::msg::AckermannLateralCommand getStopControlCommand() const; + + /** + * @brief get initial command + */ + autoware_auto_control_msgs::msg::AckermannLateralCommand getInitialControlCommand() const; + + /** + * @brief check ego car is in stopped state + */ + bool8_t isStoppedState() const; + + /** + * @brief check if the trajectory has valid value + */ + bool8_t isValidTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & traj) const; + + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; + + /** + * @brief Declare MPC parameters as ROS parameters to allow tuning on the fly + */ + void declareMPCparameters(); + + /** + * @brief Called when parameters are changed outside of node + */ + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp new file mode 100644 index 0000000000000..e66d87bb0ec22 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -0,0 +1,381 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#define TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Geometry" +#include "motion_common/motion_common.hpp" +#include "motion_common/trajectory_common.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "trajectory_follower/debug_values.hpp" +#include "trajectory_follower/longitudinal_controller_base.hpp" +#include "trajectory_follower/longitudinal_controller_utils.hpp" +#include "trajectory_follower/lowpass_filter.hpp" +#include "trajectory_follower/pid.hpp" +#include "trajectory_follower/smooth_stop.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +using autoware::common::types::bool8_t; +using autoware::common::types::float64_t; +namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; +namespace motion_common = ::autoware::motion::motion_common; + +/// \class PidLongitudinalController +/// \brief The node class used for generating longitudinal control commands (velocity/acceleration) +class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public LongitudinalControllerBase +{ +public: + explicit PidLongitudinalController(rclcpp::Node * node); + +private: + struct Motion + { + float64_t vel{0.0}; + float64_t acc{0.0}; + }; + + enum class Shift { Forward = 0, Reverse }; + + struct ControlData + { + bool8_t is_far_from_trajectory{false}; + size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx + Motion current_motion{}; + Shift shift{Shift::Forward}; // shift is used only to calculate the sign of pitch compensation + float64_t stop_dist{0.0}; // signed distance that is positive when car is before the stopline + float64_t slope_angle{0.0}; + float64_t dt{0.0}; + }; + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + // ros variables + rclcpp::Subscription::SharedPtr m_sub_current_velocity; + rclcpp::Subscription::SharedPtr m_sub_trajectory; + rclcpp::Publisher::SharedPtr + m_pub_control_cmd; + rclcpp::Publisher::SharedPtr + m_pub_slope; + rclcpp::Publisher::SharedPtr + m_pub_debug; + rclcpp::TimerBase::SharedPtr m_timer_control; + + rclcpp::Subscription::SharedPtr m_tf_sub; + rclcpp::Subscription::SharedPtr m_tf_static_sub; + tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME}; + tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; + + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + // pointers for ros topic + std::shared_ptr m_current_velocity_ptr{nullptr}; + std::shared_ptr m_prev_velocity_ptr{nullptr}; + std::shared_ptr m_trajectory_ptr{nullptr}; + + // vehicle info + float64_t m_wheel_base; + + // control state + enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY }; + ControlState m_control_state{ControlState::STOPPED}; + + // control period + float64_t m_longitudinal_ctrl_period; + + // delay compensation + float64_t m_delay_compensation_time; + + // enable flags + bool8_t m_enable_smooth_stop; + bool8_t m_enable_overshoot_emergency; + bool8_t m_enable_slope_compensation; + bool8_t m_enable_keep_stopped_until_steer_convergence; + + // smooth stop transition + struct StateTransitionParams + { + // drive + float64_t drive_state_stop_dist; + float64_t drive_state_offset_stop_dist; + // stopping + float64_t stopping_state_stop_dist; + // stop + float64_t stopped_state_entry_duration_time; + float64_t stopped_state_entry_vel; + float64_t stopped_state_entry_acc; + // emergency + float64_t emergency_state_overshoot_stop_dist; + float64_t emergency_state_traj_trans_dev; + float64_t emergency_state_traj_rot_dev; + }; + StateTransitionParams m_state_transition_params; + + // drive + trajectory_follower::PIDController m_pid_vel; + std::shared_ptr m_lpf_vel_error{nullptr}; + float64_t m_current_vel_threshold_pid_integrate; + bool8_t m_enable_brake_keeping_before_stop; + float64_t m_brake_keeping_acc; + + // smooth stop + trajectory_follower::SmoothStop m_smooth_stop; + + // stop + struct StoppedStateParams + { + float64_t vel; + float64_t acc; + float64_t jerk; + }; + StoppedStateParams m_stopped_state_params; + + // emergency + struct EmergencyStateParams + { + float64_t vel; + float64_t acc; + float64_t jerk; + }; + EmergencyStateParams m_emergency_state_params; + + // acceleration limit + float64_t m_max_acc; + float64_t m_min_acc; + + // jerk limit + float64_t m_max_jerk; + float64_t m_min_jerk; + + // slope compensation + bool8_t m_use_traj_for_pitch; + std::shared_ptr m_lpf_pitch{nullptr}; + float64_t m_max_pitch_rad; + float64_t m_min_pitch_rad; + + // 1st order lowpass filter for acceleration + std::shared_ptr m_lpf_acc{nullptr}; + + // buffer of send command + std::vector m_ctrl_cmd_vec; + + // for calculating dt + std::shared_ptr m_prev_control_time{nullptr}; + + // shift mode + Shift m_prev_shift{Shift::Forward}; + + // diff limit + Motion m_prev_ctrl_cmd{}; // with slope compensation + Motion m_prev_raw_ctrl_cmd{}; // without slope compensation + std::vector> m_vel_hist; + + // debug values + trajectory_follower::DebugValues m_debug_values; + + std::shared_ptr m_last_running_time{ + std::make_shared(rclcpp::Clock{RCL_ROS_TIME}.now())}; + + /** + * @brief set current and previous velocity with received message + * @param [in] msg current state message + */ + void callbackCurrentVelocity(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + /** + * @brief set reference trajectory with received message + * @param [in] msg trajectory message + */ + void callbackTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + + /** + * @brief compute control command, and publish periodically + */ + // void callbackTimerControl(); + LongitudinalOutput run() override; + + /** + * @brief calculate data for controllers whose type is ControlData + * @param [in] current_pose current ego pose + */ + ControlData getControlData(const geometry_msgs::msg::Pose & current_pose); + + /** + * @brief calculate control command in emergency state + * @param [in] dt time between previous and current one + */ + Motion calcEmergencyCtrlCmd(const float64_t dt) const; + + /** + * @brief update control state according to the current situation + * @param [in] current_control_state current control state + * @param [in] control_data control data + */ + ControlState updateControlState( + const ControlState current_control_state, const ControlData & control_data); + + /** + * @brief calculate control command based on the current control state + * @param [in] current_control_state current control state + * @param [in] current_pose current ego pose + * @param [in] control_data control data + */ + Motion calcCtrlCmd( + const ControlState & current_control_state, const geometry_msgs::msg::Pose & current_pose, + const ControlData & control_data); + + /** + * @brief publish control command + * @param [in] ctrl_cmd calculated control command to control velocity + * @param [in] current_vel current velocity of the vehicle + */ + autoware_auto_control_msgs::msg::LongitudinalCommand createCtrlCmdMsg( + const Motion & ctrl_cmd, const float64_t current_vel); + + /** + * @brief publish debug data + * @param [in] ctrl_cmd calculated control command to control velocity + * @param [in] control_data data for control calculation + */ + void publishDebugData(const Motion & ctrl_cmd, const ControlData & control_data); + + /** + * @brief calculate time between current and previous one + */ + float64_t getDt(); + + /** + * @brief calculate current velocity and acceleration + */ + Motion getCurrentMotion() const; + + /** + * @brief calculate direction (forward or backward) that vehicle moves + * @param [in] nearest_idx nearest index on trajectory to vehicle + */ + enum Shift getCurrentShift(const size_t nearest_idx) const; + + /** + * @brief filter acceleration command with limitation of acceleration and jerk, and slope + * compensation + * @param [in] raw_acc acceleration before filtered + * @param [in] control_data data for control calculation + */ + float64_t calcFilteredAcc(const float64_t raw_acc, const ControlData & control_data); + + /** + * @brief store acceleration command before slope compensation + * @param [in] accel command before slope compensation + */ + void storeAccelCmd(const float64_t accel); + + /** + * @brief add acceleration to compensate for slope + * @param [in] acc acceleration before slope compensation + * @param [in] pitch pitch angle (upward is negative) + * @param [in] shift direction that vehicle move (forward or backward) + */ + float64_t applySlopeCompensation( + const float64_t acc, const float64_t pitch, const Shift shift) const; + + /** + * @brief keep target motion acceleration negative before stop + * @param [in] traj reference trajectory + * @param [in] motion delay compensated target motion + */ + Motion keepBrakeBeforeStop( + const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, + const size_t nearest_idx) const; + + /** + * @brief interpolate trajectory point that is nearest to vehicle + * @param [in] traj reference trajectory + * @param [in] point vehicle position + * @param [in] nearest_idx index of the trajectory point nearest to the vehicle position + */ + autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const geometry_msgs::msg::Point & point, const size_t nearest_idx) const; + + /** + * @brief calculate predicted velocity after time delay based on past control commands + * @param [in] current_motion current velocity and acceleration of the vehicle + * @param [in] delay_compensation_time predicted time delay + */ + float64_t predictedVelocityInTargetPoint( + const Motion current_motion, const float64_t delay_compensation_time) const; + + /** + * @brief calculate velocity feedback with feed forward and pid controller + * @param [in] target_motion reference velocity and acceleration. This acceleration will be used + * as feed forward. + * @param [in] dt time step to use + * @param [in] current_vel current velocity of the vehicle + */ + float64_t applyVelocityFeedback( + const Motion target_motion, const float64_t dt, const float64_t current_vel); + + /** + * @brief update variables for debugging about pitch + * @param [in] pitch current pitch of the vehicle (filtered) + * @param [in] traj_pitch current trajectory pitch + * @param [in] raw_pitch current raw pitch of the vehicle (unfiltered) + */ + void updatePitchDebugValues( + const float64_t pitch, const float64_t traj_pitch, const float64_t raw_pitch); + + /** + * @brief update variables for velocity and acceleration + * @param [in] ctrl_cmd latest calculated control command + * @param [in] current_pose current pose of the vehicle + * @param [in] control_data data for control calculation + */ + void updateDebugVelAcc( + const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, + const ControlData & control_data); +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp b/control/trajectory_follower/include/trajectory_follower/sync_data.hpp new file mode 100644 index 0000000000000..48224a5498a9c --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/sync_data.hpp @@ -0,0 +1,41 @@ +// Copyright 2022 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 TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ +#define TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +struct LateralSyncData +{ + bool is_steer_converged{false}; +}; + +struct LongitudinalSyncData +{ + bool is_velocity_converged{false}; +}; + +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower/package.xml index 4bc005335f4a9..c68b1013ab252 100644 --- a/control/trajectory_follower/package.xml +++ b/control/trajectory_follower/package.xml @@ -27,8 +27,9 @@ std_msgs tf2 tf2_ros + vehicle_info_util - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common eigen diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp new file mode 100644 index 0000000000000..6df6ad577fb92 --- /dev/null +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -0,0 +1,549 @@ +// 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 "trajectory_follower/mpc_lateral_controller.hpp" + +#include "tf2_ros/create_timer_ros.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +namespace +{ +using namespace std::literals::chrono_literals; + +template +void update_param( + const std::vector & parameters, const std::string & name, T & value) +{ + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + value = static_cast(it->template get_value()); + } +} +} // namespace + +MpcLateralController::MpcLateralController(rclcpp::Node * node) : node_{node} +{ + using std::placeholders::_1; + + m_mpc.m_ctrl_period = node_->get_parameter("ctrl_period").as_double(); + m_enable_path_smoothing = node_->declare_parameter("enable_path_smoothing"); + m_path_filter_moving_ave_num = node_->declare_parameter("path_filter_moving_ave_num"); + m_curvature_smoothing_num_traj = + node_->declare_parameter("curvature_smoothing_num_traj"); + m_curvature_smoothing_num_ref_steer = + node_->declare_parameter("curvature_smoothing_num_ref_steer"); + m_traj_resample_dist = node_->declare_parameter("traj_resample_dist"); + m_mpc.m_admissible_position_error = + node_->declare_parameter("admissible_position_error"); + m_mpc.m_admissible_yaw_error_rad = + node_->declare_parameter("admissible_yaw_error_rad"); + m_mpc.m_use_steer_prediction = node_->declare_parameter("use_steer_prediction"); + m_mpc.m_param.steer_tau = node_->declare_parameter("vehicle_model_steer_tau"); + + /* stop state parameters */ + m_stop_state_entry_ego_speed = node_->declare_parameter("stop_state_entry_ego_speed"); + m_stop_state_entry_target_speed = + node_->declare_parameter("stop_state_entry_target_speed"); + m_converged_steer_rad = node_->declare_parameter("converged_steer_rad"); + m_check_steer_converged_for_stopped_state = + node_->declare_parameter("check_steer_converged_for_stopped_state"); + + /* mpc parameters */ + const float64_t steer_lim_deg = node_->declare_parameter("steer_lim_deg"); + const float64_t steer_rate_lim_dps = node_->declare_parameter("steer_rate_lim_dps"); + constexpr float64_t deg2rad = static_cast(autoware::common::types::PI) / 180.0; + m_mpc.m_steer_lim = steer_lim_deg * deg2rad; + m_mpc.m_steer_rate_lim = steer_rate_lim_dps * deg2rad; + const float64_t wheelbase = + vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo().wheel_base_m; + + /* vehicle model setup */ + const std::string vehicle_model_type = + node_->declare_parameter("vehicle_model_type"); + std::shared_ptr vehicle_model_ptr; + if (vehicle_model_type == "kinematics") { + vehicle_model_ptr = std::make_shared( + wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau); + } else if (vehicle_model_type == "kinematics_no_delay") { + vehicle_model_ptr = std::make_shared( + wheelbase, m_mpc.m_steer_lim); + } else if (vehicle_model_type == "dynamics") { + const float64_t mass_fl = node_->declare_parameter("vehicle.mass_fl"); + const float64_t mass_fr = node_->declare_parameter("vehicle.mass_fr"); + const float64_t mass_rl = node_->declare_parameter("vehicle.mass_rl"); + const float64_t mass_rr = node_->declare_parameter("vehicle.mass_rr"); + const float64_t cf = node_->declare_parameter("vehicle.cf"); + const float64_t cr = node_->declare_parameter("vehicle.cr"); + + // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time + // // NOLINT + vehicle_model_ptr = std::make_shared( + wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); + } else { + RCLCPP_ERROR(node_->get_logger(), "vehicle_model_type is undefined"); + } + + /* QP solver setup */ + const std::string qp_solver_type = node_->declare_parameter("qp_solver_type"); + std::shared_ptr qpsolver_ptr; + if (qp_solver_type == "unconstraint_fast") { + qpsolver_ptr = std::make_shared(); + } else if (qp_solver_type == "osqp") { + qpsolver_ptr = std::make_shared(node_->get_logger()); + } else { + RCLCPP_ERROR(node_->get_logger(), "qp_solver_type is undefined"); + } + + /* delay compensation */ + { + const float64_t delay_tmp = node_->declare_parameter("input_delay"); + const float64_t delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period); + m_mpc.m_param.input_delay = delay_step * m_mpc.m_ctrl_period; + m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + } + + /* initialize lowpass filter */ + { + const float64_t steering_lpf_cutoff_hz = + node_->declare_parameter("steering_lpf_cutoff_hz"); + const float64_t error_deriv_lpf_cutoff_hz = + node_->declare_parameter("error_deriv_lpf_cutoff_hz"); + m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + } + + /* set up ros system */ + // initTimer(m_mpc.m_ctrl_period); + + m_pub_ctrl_cmd = + node_->create_publisher( + "~/output/lateral_control_cmd", 1); + m_pub_predicted_traj = node_->create_publisher( + "~/output/predicted_trajectory", 1); + m_pub_diagnostic = + node_->create_publisher( + "~/output/lateral_diagnostic", 1); + m_sub_ref_path = node_->create_subscription( + "~/input/current_trajectory", rclcpp::QoS{1}, + std::bind(&MpcLateralController::onTrajectory, this, _1)); + m_sub_steering = node_->create_subscription( + "~/input/current_steering", rclcpp::QoS{1}, + std::bind(&MpcLateralController::onSteering, this, _1)); + m_sub_odometry = node_->create_subscription( + "~/input/current_odometry", rclcpp::QoS{1}, + std::bind(&MpcLateralController::onOdometry, this, _1)); + + // TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations + declareMPCparameters(); + + /* get parameter updates */ + m_set_param_res = node_->add_on_set_parameters_callback( + std::bind(&MpcLateralController::paramCallback, this, _1)); + + m_mpc.setQPSolver(qpsolver_ptr); + m_mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + + m_mpc.setLogger(node_->get_logger()); + m_mpc.setClock(node_->get_clock()); +} + +MpcLateralController::~MpcLateralController() +{ + autoware_auto_control_msgs::msg::AckermannLateralCommand stop_cmd = getStopControlCommand(); + createCtrlCmdMsg(stop_cmd); // todo +} + +LateralOutput MpcLateralController::run() +{ + if (!checkData() || !updateCurrentPose()) { + LateralOutput output; + return output; // todo + } + + autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd; + autoware_auto_planning_msgs::msg::Trajectory predicted_traj; + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic diagnostic; + + if (!m_is_ctrl_cmd_prev_initialized) { + m_ctrl_cmd_prev = getInitialControlCommand(); + m_is_ctrl_cmd_prev_initialized = true; + } + + const bool8_t is_mpc_solved = m_mpc.calculateMPC( + *m_current_steering_ptr, m_current_odometry_ptr->twist.twist.linear.x, m_current_pose_ptr->pose, + ctrl_cmd, predicted_traj, diagnostic); + + if (isStoppedState()) { + // Reset input buffer + for (auto & value : m_mpc.m_input_buffer) { + value = m_ctrl_cmd_prev.steering_tire_angle; + } + // Use previous command value as previous raw steer command + m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; + + const auto cmd_msg = createCtrlCmdMsg(m_ctrl_cmd_prev); + publishPredictedTraj(predicted_traj); + publishDiagnostic(diagnostic); + LateralOutput output; + output.control_cmd = cmd_msg; + output.sync_data.is_steer_converged = + std::abs(cmd_msg.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + m_converged_steer_rad; + return output; + } + + if (!is_mpc_solved) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, + "MPC is not solved. publish 0 velocity."); + ctrl_cmd = getStopControlCommand(); + } + + m_ctrl_cmd_prev = ctrl_cmd; + const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd); + publishPredictedTraj(predicted_traj); + publishDiagnostic(diagnostic); + LateralOutput output; + output.control_cmd = cmd_msg; + output.sync_data.is_steer_converged = + std::abs(cmd_msg.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + m_converged_steer_rad; + return output; +} + +bool8_t MpcLateralController::checkData() const +{ + if (!m_mpc.hasVehicleModel()) { + RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a vehicle model"); + return false; + } + if (!m_mpc.hasQPSolver()) { + RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a QP solver"); + return false; + } + + if (!m_current_odometry_ptr) { + RCLCPP_DEBUG( + node_->get_logger(), "waiting data. current_velocity = %d", + m_current_odometry_ptr != nullptr); + return false; + } + + if (!m_current_steering_ptr) { + RCLCPP_DEBUG( + node_->get_logger(), "waiting data. current_steering = %d", + m_current_steering_ptr != nullptr); + return false; + } + + if (m_mpc.m_ref_traj.size() == 0) { + RCLCPP_DEBUG(node_->get_logger(), "trajectory size is zero."); + return false; + } + + return true; +} + +void MpcLateralController::onTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + m_current_trajectory_ptr = msg; + + if (!m_current_pose_ptr && !updateCurrentPose()) { + RCLCPP_DEBUG(node_->get_logger(), "Current pose is not received yet."); + return; + } + + if (msg->points.size() < 3) { + RCLCPP_DEBUG(node_->get_logger(), "received path size is < 3, not enough."); + return; + } + + if (!isValidTrajectory(*msg)) { + RCLCPP_ERROR(node_->get_logger(), "Trajectory is invalid!! stop computing."); + return; + } + + m_mpc.setReferenceTrajectory( + *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num, + m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, m_current_pose_ptr); +} + +bool8_t MpcLateralController::updateCurrentPose() +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = m_tf_buffer.lookupTransform( + m_current_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, ex.what()); + RCLCPP_WARN_SKIPFIRST_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, + m_tf_buffer.allFramesAsString().c_str()); + return false; + } + + geometry_msgs::msg::PoseStamped ps; + ps.header = transform.header; + ps.pose.position.x = transform.transform.translation.x; + ps.pose.position.y = transform.transform.translation.y; + ps.pose.position.z = transform.transform.translation.z; + ps.pose.orientation = transform.transform.rotation; + m_current_pose_ptr = std::make_shared(ps); + return true; +} + +void MpcLateralController::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + m_current_odometry_ptr = msg; +} + +void MpcLateralController::onSteering( + const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) +{ + m_current_steering_ptr = msg; +} + +autoware_auto_control_msgs::msg::AckermannLateralCommand +MpcLateralController::getStopControlCommand() const +{ + autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + cmd.steering_tire_angle = static_cast(m_steer_cmd_prev); + cmd.steering_tire_rotation_rate = 0.0; + return cmd; +} + +autoware_auto_control_msgs::msg::AckermannLateralCommand +MpcLateralController::getInitialControlCommand() const +{ + autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + cmd.steering_tire_angle = m_current_steering_ptr->steering_tire_angle; + cmd.steering_tire_rotation_rate = 0.0; + return cmd; +} + +bool8_t MpcLateralController::isStoppedState() const +{ + // Note: This function used to take into account the distance to the stop line + // for the stop state judgement. However, it has been removed since the steering + // control was turned off when approaching/exceeding the stop line on a curve or + // emergency stop situation and it caused large tracking error. + const int64_t nearest = trajectory_follower::MPCUtils::calcNearestIndex( + *m_current_trajectory_ptr, m_current_pose_ptr->pose); + + // If the nearest index is not found, return false + if (nearest < 0) { + return false; + } + + const float64_t current_vel = m_current_odometry_ptr->twist.twist.linear.x; + const float64_t target_vel = + m_current_trajectory_ptr->points.at(static_cast(nearest)).longitudinal_velocity_mps; + if ( + std::fabs(current_vel) < m_stop_state_entry_ego_speed && + std::fabs(target_vel) < m_stop_state_entry_target_speed && + (!m_check_steer_converged_for_stopped_state || + std::abs(m_ctrl_cmd_prev.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + m_converged_steer_rad)) { + return true; + } else { + return false; + } +} + +autoware_auto_control_msgs::msg::AckermannLateralCommand MpcLateralController::createCtrlCmdMsg( + autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd) +{ + ctrl_cmd.stamp = node_->now(); + // m_pub_ctrl_cmd->publish(ctrl_cmd); + m_steer_cmd_prev = ctrl_cmd.steering_tire_angle; + return ctrl_cmd; +} + +void MpcLateralController::publishPredictedTraj( + autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const +{ + predicted_traj.header.stamp = node_->now(); + predicted_traj.header.frame_id = m_current_trajectory_ptr->header.frame_id; + m_pub_predicted_traj->publish(predicted_traj); +} + +void MpcLateralController::publishDiagnostic( + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) const +{ + diagnostic.diag_header.data_stamp = node_->now(); + diagnostic.diag_header.name = std::string("linear-MPC lateral controller"); + m_pub_diagnostic->publish(diagnostic); +} + +// void MpcLateralController::initTimer(float64_t period_s) +// { +// const auto period_ns = std::chrono::duration_cast( +// std::chrono::duration(period_s)); +// m_timer = rclcpp::create_timer( +// this, node_->get_clock(), period_ns, std::bind(&MpcLateralController::onTimer, this)); +// } + +void MpcLateralController::declareMPCparameters() +{ + m_mpc.m_param.prediction_horizon = node_->declare_parameter("mpc_prediction_horizon"); + m_mpc.m_param.prediction_dt = node_->declare_parameter("mpc_prediction_dt"); + m_mpc.m_param.weight_lat_error = node_->declare_parameter("mpc_weight_lat_error"); + m_mpc.m_param.weight_heading_error = + node_->declare_parameter("mpc_weight_heading_error"); + m_mpc.m_param.weight_heading_error_squared_vel = + node_->declare_parameter("mpc_weight_heading_error_squared_vel"); + m_mpc.m_param.weight_steering_input = + node_->declare_parameter("mpc_weight_steering_input"); + m_mpc.m_param.weight_steering_input_squared_vel = + node_->declare_parameter("mpc_weight_steering_input_squared_vel"); + m_mpc.m_param.weight_lat_jerk = node_->declare_parameter("mpc_weight_lat_jerk"); + m_mpc.m_param.weight_steer_rate = node_->declare_parameter("mpc_weight_steer_rate"); + m_mpc.m_param.weight_steer_acc = node_->declare_parameter("mpc_weight_steer_acc"); + m_mpc.m_param.low_curvature_weight_lat_error = + node_->declare_parameter("mpc_low_curvature_weight_lat_error"); + m_mpc.m_param.low_curvature_weight_heading_error = + node_->declare_parameter("mpc_low_curvature_weight_heading_error"); + m_mpc.m_param.low_curvature_weight_heading_error_squared_vel = + node_->declare_parameter("mpc_low_curvature_weight_heading_error_squared_vel"); + m_mpc.m_param.low_curvature_weight_steering_input = + node_->declare_parameter("mpc_low_curvature_weight_steering_input"); + m_mpc.m_param.low_curvature_weight_steering_input_squared_vel = + node_->declare_parameter("mpc_low_curvature_weight_steering_input_squared_vel"); + m_mpc.m_param.low_curvature_weight_lat_jerk = + node_->declare_parameter("mpc_low_curvature_weight_lat_jerk"); + m_mpc.m_param.low_curvature_weight_steer_rate = + node_->declare_parameter("mpc_low_curvature_weight_steer_rate"); + m_mpc.m_param.low_curvature_weight_steer_acc = + node_->declare_parameter("mpc_low_curvature_weight_steer_acc"); + m_mpc.m_param.low_curvature_thresh_curvature = + node_->declare_parameter("mpc_low_curvature_thresh_curvature"); + m_mpc.m_param.weight_terminal_lat_error = + node_->declare_parameter("mpc_weight_terminal_lat_error"); + m_mpc.m_param.weight_terminal_heading_error = + node_->declare_parameter("mpc_weight_terminal_heading_error"); + m_mpc.m_param.zero_ff_steer_deg = node_->declare_parameter("mpc_zero_ff_steer_deg"); + m_mpc.m_param.acceleration_limit = node_->declare_parameter("mpc_acceleration_limit"); + m_mpc.m_param.velocity_time_constant = + node_->declare_parameter("mpc_velocity_time_constant"); +} + +rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + // strong exception safety wrt MPCParam + trajectory_follower::MPCParam param = m_mpc.m_param; + try { + update_param(parameters, "mpc_prediction_horizon", param.prediction_horizon); + update_param(parameters, "mpc_prediction_dt", param.prediction_dt); + update_param(parameters, "mpc_weight_lat_error", param.weight_lat_error); + update_param(parameters, "mpc_weight_heading_error", param.weight_heading_error); + update_param( + parameters, "mpc_weight_heading_error_squared_vel", param.weight_heading_error_squared_vel); + update_param(parameters, "mpc_weight_steering_input", param.weight_steering_input); + update_param( + parameters, "mpc_weight_steering_input_squared_vel", param.weight_steering_input_squared_vel); + update_param(parameters, "mpc_weight_lat_jerk", param.weight_lat_jerk); + update_param(parameters, "mpc_weight_steer_rate", param.weight_steer_rate); + update_param(parameters, "mpc_weight_steer_acc", param.weight_steer_acc); + update_param( + parameters, "mpc_low_curvature_weight_lat_error", param.low_curvature_weight_lat_error); + update_param( + parameters, "mpc_low_curvature_weight_heading_error", + param.low_curvature_weight_heading_error); + update_param( + parameters, "mpc_low_curvature_weight_heading_error_squared_vel", + param.low_curvature_weight_heading_error_squared_vel); + update_param( + parameters, "mpc_low_curvature_weight_steering_input", + param.low_curvature_weight_steering_input); + update_param( + parameters, "mpc_low_curvature_weight_steering_input_squared_vel", + param.low_curvature_weight_steering_input_squared_vel); + update_param( + parameters, "mpc_low_curvature_weight_lat_jerk", param.low_curvature_weight_lat_jerk); + update_param( + parameters, "mpc_low_curvature_weight_steer_rate", param.low_curvature_weight_steer_rate); + update_param( + parameters, "mpc_low_curvature_weight_steer_acc", param.low_curvature_weight_steer_acc); + update_param( + parameters, "mpc_low_curvature_thresh_curvature", param.low_curvature_thresh_curvature); + update_param(parameters, "mpc_weight_terminal_lat_error", param.weight_terminal_lat_error); + update_param( + parameters, "mpc_weight_terminal_heading_error", param.weight_terminal_heading_error); + update_param(parameters, "mpc_zero_ff_steer_deg", param.zero_ff_steer_deg); + update_param(parameters, "mpc_acceleration_limit", param.acceleration_limit); + update_param(parameters, "mpc_velocity_time_constant", param.velocity_time_constant); + + // initialize input buffer + update_param(parameters, "input_delay", param.input_delay); + const float64_t delay_step = std::round(param.input_delay / m_mpc.m_ctrl_period); + const float64_t delay = delay_step * m_mpc.m_ctrl_period; + if (param.input_delay != delay) { + param.input_delay = delay; + m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + } + + // transaction succeeds, now assign values + m_mpc.m_param = param; + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + } + + return result; +} + +bool8_t MpcLateralController::isValidTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & traj) const +{ + for (const auto & p : traj.points) { + if ( + !isfinite(p.pose.position.x) || !isfinite(p.pose.position.y) || + !isfinite(p.pose.orientation.w) || !isfinite(p.pose.orientation.x) || + !isfinite(p.pose.orientation.y) || !isfinite(p.pose.orientation.z) || + !isfinite(p.longitudinal_velocity_mps) || !isfinite(p.lateral_velocity_mps) || + !isfinite(p.lateral_velocity_mps) || !isfinite(p.heading_rate_rps) || + !isfinite(p.front_wheel_angle_rad) || !isfinite(p.rear_wheel_angle_rad)) { + return false; + } + } + return true; +} + +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp new file mode 100644 index 0000000000000..80c84475b5552 --- /dev/null +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -0,0 +1,963 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "trajectory_follower/pid_longitudinal_controller.hpp" + +#include "motion_common/motion_common.hpp" +#include "motion_common/trajectory_common.hpp" +#include "time_utils/time_utils.hpp" + +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +PidLongitudinalController::PidLongitudinalController(rclcpp::Node * node) +: node_{node}, logger_{node->get_logger()}, clock_{node->get_clock()} +{ + using std::placeholders::_1; + + // parameters timer + m_longitudinal_ctrl_period = node_->get_parameter("ctrl_period").as_double(); + + m_wheel_base = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo().wheel_base_m; + + // parameters for delay compensation + m_delay_compensation_time = + node_->declare_parameter("delay_compensation_time"); // [s] + + // parameters to enable functions + m_enable_smooth_stop = node_->declare_parameter("enable_smooth_stop"); + m_enable_overshoot_emergency = node_->declare_parameter("enable_overshoot_emergency"); + m_enable_slope_compensation = node_->declare_parameter("enable_slope_compensation"); + m_enable_keep_stopped_until_steer_convergence = + node_->declare_parameter("enable_keep_stopped_until_steer_convergence"); + + // parameters for state transition + { + auto & p = m_state_transition_params; + // drive + p.drive_state_stop_dist = node_->declare_parameter("drive_state_stop_dist"); // [m] + p.drive_state_offset_stop_dist = + node_->declare_parameter("drive_state_offset_stop_dist"); // [m] + // stopping + p.stopping_state_stop_dist = + node_->declare_parameter("stopping_state_stop_dist"); // [m] + p.stopped_state_entry_duration_time = + node_->declare_parameter("stopped_state_entry_duration_time"); // [s] + // stop + p.stopped_state_entry_vel = + node_->declare_parameter("stopped_state_entry_vel"); // [m/s] + p.stopped_state_entry_acc = + node_->declare_parameter("stopped_state_entry_acc"); // [m/s²] + // emergency + p.emergency_state_overshoot_stop_dist = + node_->declare_parameter("emergency_state_overshoot_stop_dist"); // [m] + p.emergency_state_traj_trans_dev = + node_->declare_parameter("emergency_state_traj_trans_dev"); // [m] + p.emergency_state_traj_rot_dev = + node_->declare_parameter("emergency_state_traj_rot_dev"); // [m] + } + + // parameters for drive state + { + // initialize PID gain + const float64_t kp{node_->declare_parameter("kp")}; + const float64_t ki{node_->declare_parameter("ki")}; + const float64_t kd{node_->declare_parameter("kd")}; + m_pid_vel.setGains(kp, ki, kd); + + // initialize PID limits + const float64_t max_pid{node_->declare_parameter("max_out")}; // [m/s^2] + const float64_t min_pid{node_->declare_parameter("min_out")}; // [m/s^2] + const float64_t max_p{node_->declare_parameter("max_p_effort")}; // [m/s^2] + const float64_t min_p{node_->declare_parameter("min_p_effort")}; // [m/s^2] + const float64_t max_i{node_->declare_parameter("max_i_effort")}; // [m/s^2] + const float64_t min_i{node_->declare_parameter("min_i_effort")}; // [m/s^2] + const float64_t max_d{node_->declare_parameter("max_d_effort")}; // [m/s^2] + const float64_t min_d{node_->declare_parameter("min_d_effort")}; // [m/s^2] + m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); + + // set lowpass filter for vel error and pitch + const float64_t lpf_vel_error_gain{node_->declare_parameter("lpf_vel_error_gain")}; + m_lpf_vel_error = + std::make_shared(0.0, lpf_vel_error_gain); + + m_current_vel_threshold_pid_integrate = + node_->declare_parameter("current_vel_threshold_pid_integration"); // [m/s] + + m_enable_brake_keeping_before_stop = + node_->declare_parameter("enable_brake_keeping_before_stop"); // [-] + m_brake_keeping_acc = node_->declare_parameter("brake_keeping_acc"); // [m/s^2] + } + + // parameters for smooth stop state + { + const float64_t max_strong_acc{ + node_->declare_parameter("smooth_stop_max_strong_acc")}; // [m/s^2] + const float64_t min_strong_acc{ + node_->declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] + const float64_t weak_acc{ + node_->declare_parameter("smooth_stop_weak_acc")}; // [m/s^2] + const float64_t weak_stop_acc{ + node_->declare_parameter("smooth_stop_weak_stop_acc")}; // [m/s^2] + const float64_t strong_stop_acc{ + node_->declare_parameter("smooth_stop_strong_stop_acc")}; // [m/s^2] + + const float64_t max_fast_vel{ + node_->declare_parameter("smooth_stop_max_fast_vel")}; // [m/s] + const float64_t min_running_vel{ + node_->declare_parameter("smooth_stop_min_running_vel")}; // [m/s] + const float64_t min_running_acc{ + node_->declare_parameter("smooth_stop_min_running_acc")}; // [m/s^2] + const float64_t weak_stop_time{ + node_->declare_parameter("smooth_stop_weak_stop_time")}; // [s] + + const float64_t weak_stop_dist{ + node_->declare_parameter("smooth_stop_weak_stop_dist")}; // [m] + const float64_t strong_stop_dist{ + node_->declare_parameter("smooth_stop_strong_stop_dist")}; // [m] + + m_smooth_stop.setParams( + max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, + min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist); + } + + // parameters for stop state + { + auto & p = m_stopped_state_params; + p.vel = node_->declare_parameter("stopped_vel"); // [m/s] + p.acc = node_->declare_parameter("stopped_acc"); // [m/s^2] + p.jerk = node_->declare_parameter("stopped_jerk"); // [m/s^3] + } + + // parameters for emergency state + { + auto & p = m_emergency_state_params; + p.vel = node_->declare_parameter("emergency_vel"); // [m/s] + p.acc = node_->declare_parameter("emergency_acc"); // [m/s^2] + p.jerk = node_->declare_parameter("emergency_jerk"); // [m/s^3] + } + + // parameters for acceleration limit + m_max_acc = node_->declare_parameter("max_acc"); // [m/s^2] + m_min_acc = node_->declare_parameter("min_acc"); // [m/s^2] + + // parameters for jerk limit + m_max_jerk = node_->declare_parameter("max_jerk"); // [m/s^3] + m_min_jerk = node_->declare_parameter("min_jerk"); // [m/s^3] + + // parameters for slope compensation + m_use_traj_for_pitch = node_->declare_parameter("use_trajectory_for_pitch_calculation"); + const float64_t lpf_pitch_gain{node_->declare_parameter("lpf_pitch_gain")}; + m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); + m_max_pitch_rad = node_->declare_parameter("max_pitch_rad"); // [rad] + m_min_pitch_rad = node_->declare_parameter("min_pitch_rad"); // [rad] + + // subscriber, publisher + m_sub_current_velocity = node_->create_subscription( + "~/input/current_odometry", rclcpp::QoS{1}, + std::bind(&PidLongitudinalController::callbackCurrentVelocity, this, _1)); + m_sub_trajectory = node_->create_subscription( + "~/input/current_trajectory", rclcpp::QoS{1}, + std::bind(&PidLongitudinalController::callbackTrajectory, this, _1)); + m_pub_control_cmd = node_->create_publisher( + "~/output/longitudinal_control_cmd", rclcpp::QoS{1}); + m_pub_slope = + node_->create_publisher( + "~/output/slope_angle", rclcpp::QoS{1}); + m_pub_debug = + node_->create_publisher( + "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); + + // Timer + // { + // const auto period_ns = std::chrono::duration_cast( + // std::chrono::duration(m_longitudinal_ctrl_period)); + // m_timer_control = rclcpp::create_timer( + // this, get_clock(), period_ns, std::bind(&PidLongitudinalController::callbackTimerControl, + // this)); + // } + + // set parameter callback + m_set_param_res = node_->add_on_set_parameters_callback( + std::bind(&PidLongitudinalController::paramCallback, this, _1)); + + // set lowpass filter for acc + m_lpf_acc = std::make_shared(0.0, 0.2); +} + +void PidLongitudinalController::callbackCurrentVelocity( + const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + if (m_current_velocity_ptr) { + m_prev_velocity_ptr = m_current_velocity_ptr; + } + m_current_velocity_ptr = std::make_shared(*msg); +} + +void PidLongitudinalController::callbackTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) +{ + if (!trajectory_follower::longitudinal_utils::isValidTrajectory(*msg)) { + RCLCPP_ERROR_THROTTLE(logger_, *clock_, 3000, "received invalid trajectory. ignore."); + return; + } + + if (msg->points.size() < 2) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 3000, "Unexpected trajectory size < 2. Ignored."); + return; + } + + m_trajectory_ptr = std::make_shared(*msg); +} + +rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback( + const std::vector & parameters) +{ + auto update_param = [&](const std::string & name, float64_t & v) { + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + v = it->as_double(); + return true; + } + return false; + }; + + // delay compensation + update_param("delay_compensation_time", m_delay_compensation_time); + + // state transition + { + auto & p = m_state_transition_params; + update_param("drive_state_stop_dist", p.drive_state_stop_dist); + update_param("stopping_state_stop_dist", p.stopping_state_stop_dist); + update_param("stopped_state_entry_duration_time", p.stopped_state_entry_duration_time); + update_param("stopped_state_entry_vel", p.stopped_state_entry_vel); + update_param("stopped_state_entry_acc", p.stopped_state_entry_acc); + update_param("emergency_state_overshoot_stop_dist", p.emergency_state_overshoot_stop_dist); + update_param("emergency_state_traj_trans_dev", p.emergency_state_traj_trans_dev); + update_param("emergency_state_traj_rot_dev", p.emergency_state_traj_rot_dev); + } + + // drive state + { + float64_t kp{node_->get_parameter("kp").as_double()}; + float64_t ki{node_->get_parameter("ki").as_double()}; + float64_t kd{node_->get_parameter("kd").as_double()}; + update_param("kp", kp); + update_param("ki", ki); + update_param("kd", kd); + m_pid_vel.setGains(kp, ki, kd); + + float64_t max_pid{node_->get_parameter("max_out").as_double()}; + float64_t min_pid{node_->get_parameter("min_out").as_double()}; + float64_t max_p{node_->get_parameter("max_p_effort").as_double()}; + float64_t min_p{node_->get_parameter("min_p_effort").as_double()}; + float64_t max_i{node_->get_parameter("max_i_effort").as_double()}; + float64_t min_i{node_->get_parameter("min_i_effort").as_double()}; + float64_t max_d{node_->get_parameter("max_d_effort").as_double()}; + float64_t min_d{node_->get_parameter("min_d_effort").as_double()}; + update_param("max_out", max_pid); + update_param("min_out", min_pid); + update_param("max_p_effort", max_p); + update_param("min_p_effort", min_p); + update_param("max_i_effort", max_i); + update_param("min_i_effort", min_i); + update_param("max_d_effort", max_d); + update_param("min_d_effort", min_d); + m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); + + update_param("current_vel_threshold_pid_integration", m_current_vel_threshold_pid_integrate); + } + + // stopping state + { + float64_t max_strong_acc{node_->get_parameter("smooth_stop_max_strong_acc").as_double()}; + float64_t min_strong_acc{node_->get_parameter("smooth_stop_min_strong_acc").as_double()}; + float64_t weak_acc{node_->get_parameter("smooth_stop_weak_acc").as_double()}; + float64_t weak_stop_acc{node_->get_parameter("smooth_stop_weak_stop_acc").as_double()}; + float64_t strong_stop_acc{node_->get_parameter("smooth_stop_strong_stop_acc").as_double()}; + float64_t max_fast_vel{node_->get_parameter("smooth_stop_max_fast_vel").as_double()}; + float64_t min_running_vel{node_->get_parameter("smooth_stop_min_running_vel").as_double()}; + float64_t min_running_acc{node_->get_parameter("smooth_stop_min_running_acc").as_double()}; + float64_t weak_stop_time{node_->get_parameter("smooth_stop_weak_stop_time").as_double()}; + float64_t weak_stop_dist{node_->get_parameter("smooth_stop_weak_stop_dist").as_double()}; + float64_t strong_stop_dist{node_->get_parameter("smooth_stop_strong_stop_dist").as_double()}; + update_param("smooth_stop_max_strong_acc", max_strong_acc); + update_param("smooth_stop_min_strong_acc", min_strong_acc); + update_param("smooth_stop_weak_acc", weak_acc); + update_param("smooth_stop_weak_stop_acc", weak_stop_acc); + update_param("smooth_stop_strong_stop_acc", strong_stop_acc); + update_param("smooth_stop_max_fast_vel", max_fast_vel); + update_param("smooth_stop_min_running_vel", min_running_vel); + update_param("smooth_stop_min_running_acc", min_running_acc); + update_param("smooth_stop_weak_stop_time", weak_stop_time); + update_param("smooth_stop_weak_stop_dist", weak_stop_dist); + update_param("smooth_stop_strong_stop_dist", strong_stop_dist); + m_smooth_stop.setParams( + max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, + min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist); + } + + // stop state + { + auto & p = m_stopped_state_params; + update_param("stopped_vel", p.vel); + update_param("stopped_acc", p.acc); + update_param("stopped_jerk", p.jerk); + } + + // emergency state + { + auto & p = m_emergency_state_params; + update_param("emergency_vel", p.vel); + update_param("emergency_acc", p.acc); + update_param("emergency_jerk", p.jerk); + } + + // acceleration limit + update_param("min_acc", m_min_acc); + + // jerk limit + update_param("max_jerk", m_max_jerk); + update_param("min_jerk", m_min_jerk); + + // slope compensation + update_param("max_pitch_rad", m_max_pitch_rad); + update_param("min_pitch_rad", m_min_pitch_rad); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +LongitudinalOutput PidLongitudinalController::run() +{ + // wait for initial pointers + if ( + !m_current_velocity_ptr || !m_prev_velocity_ptr || !m_trajectory_ptr || + !m_tf_buffer.canTransform(m_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero)) { + LongitudinalOutput output; + return output; // todo + } + + // get current ego pose + geometry_msgs::msg::TransformStamped tf = + m_tf_buffer.lookupTransform(m_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero); + + // calculate current pose and control data + geometry_msgs::msg::Pose current_pose; + current_pose.position.x = tf.transform.translation.x; + current_pose.position.y = tf.transform.translation.y; + current_pose.position.z = tf.transform.translation.z; + current_pose.orientation = tf.transform.rotation; + + const auto control_data = getControlData(current_pose); + + // self pose is far from trajectory + if (control_data.is_far_from_trajectory) { + m_control_state = ControlState::EMERGENCY; // update control state + const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); // calculate control command + m_prev_raw_ctrl_cmd = raw_ctrl_cmd; + const auto cmd_msg = + createCtrlCmdMsg(raw_ctrl_cmd, control_data.current_motion.vel); // create control command + publishDebugData(raw_ctrl_cmd, control_data); // publish debug data + LongitudinalOutput output; + output.control_cmd = cmd_msg; + return output; + } + + // update control state + m_control_state = updateControlState(m_control_state, control_data); + + // calculate control command + const Motion ctrl_cmd = calcCtrlCmd(m_control_state, current_pose, control_data); + + // publish control command + const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd, control_data.current_motion.vel); + LongitudinalOutput output; + output.control_cmd = cmd_msg; + + // publish debug data + publishDebugData(ctrl_cmd, control_data); + + return output; +} + +PidLongitudinalController::ControlData PidLongitudinalController::getControlData( + const geometry_msgs::msg::Pose & current_pose) +{ + ControlData control_data{}; + + // dt + control_data.dt = getDt(); + + // current velocity and acceleration + control_data.current_motion = getCurrentMotion(); + + // nearest idx + const float64_t max_dist = m_state_transition_params.emergency_state_traj_trans_dev; + const float64_t max_yaw = m_state_transition_params.emergency_state_traj_rot_dev; + const auto nearest_idx_opt = + motion_common::findNearestIndex(m_trajectory_ptr->points, current_pose, max_dist, max_yaw); + + // return here if nearest index is not found + if (!nearest_idx_opt) { + control_data.is_far_from_trajectory = true; + return control_data; + } + control_data.nearest_idx = *nearest_idx_opt; + + // shift + control_data.shift = getCurrentShift(control_data.nearest_idx); + if (control_data.shift != m_prev_shift) { + m_pid_vel.reset(); + } + m_prev_shift = control_data.shift; + + // distance to stopline + control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance( + current_pose.position, *m_trajectory_ptr); + + // pitch + const float64_t raw_pitch = + trajectory_follower::longitudinal_utils::getPitchByPose(current_pose.orientation); + const float64_t traj_pitch = trajectory_follower::longitudinal_utils::getPitchByTraj( + *m_trajectory_ptr, control_data.nearest_idx, m_wheel_base); + control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); + updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); + + return control_data; +} + +PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCmd( + const float64_t dt) const +{ + // These accelerations are without slope compensation + const auto & p = m_emergency_state_params; + const float64_t vel = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); + const float64_t acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); + + auto clock = rclcpp::Clock{RCL_ROS_TIME}; + RCLCPP_ERROR_THROTTLE(logger_, clock, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc); + + return Motion{vel, acc}; +} + +PidLongitudinalController::ControlState PidLongitudinalController::updateControlState( + const ControlState current_control_state, const ControlData & control_data) +{ + const float64_t current_vel = control_data.current_motion.vel; + const float64_t current_acc = control_data.current_motion.acc; + const float64_t stop_dist = control_data.stop_dist; + + // flags for state transition + const auto & p = m_state_transition_params; + + const bool8_t departure_condition_from_stopping = + stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist; + const bool8_t departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist; + const bool8_t keep_stopped_condition = !lateral_sync_data_.is_steer_converged; + + const bool8_t stopping_condition = stop_dist < p.stopping_state_stop_dist; + if ( + std::fabs(current_vel) > p.stopped_state_entry_vel || + std::fabs(current_acc) > p.stopped_state_entry_acc) { + m_last_running_time = std::make_shared(rclcpp::Clock{RCL_ROS_TIME}.now()); + } + const bool8_t stopped_condition = + m_last_running_time ? (rclcpp::Clock{RCL_ROS_TIME}.now() - *m_last_running_time).seconds() > + p.stopped_state_entry_duration_time + : false; + + const bool8_t emergency_condition = + m_enable_overshoot_emergency && stop_dist < -p.emergency_state_overshoot_stop_dist; + + // transit state + if (current_control_state == ControlState::DRIVE) { + if (emergency_condition) { + return ControlState::EMERGENCY; + } + + if (m_enable_smooth_stop) { + if (stopping_condition) { + // predictions after input time delay + const float64_t pred_vel_in_target = + predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); + const float64_t pred_stop_dist = + control_data.stop_dist - + 0.5 * (pred_vel_in_target + current_vel) * m_delay_compensation_time; + m_smooth_stop.init(pred_vel_in_target, pred_stop_dist); + return ControlState::STOPPING; + } + } else { + if (stopped_condition && !departure_condition_from_stopped) { + return ControlState::STOPPED; + } + } + } else if (current_control_state == ControlState::STOPPING) { + if (emergency_condition) { + return ControlState::EMERGENCY; + } + + if (stopped_condition) { + return ControlState::STOPPED; + } + + if (departure_condition_from_stopping) { + m_pid_vel.reset(); + m_lpf_vel_error->reset(0.0); + // prevent the car from taking a long time to start to move + m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc); + return ControlState::DRIVE; + } + } else if (current_control_state == ControlState::STOPPED) { + if ( + departure_condition_from_stopped && + (!m_enable_keep_stopped_until_steer_convergence || !keep_stopped_condition)) { + m_pid_vel.reset(); + m_lpf_vel_error->reset(0.0); + // prevent the car from taking a long time to start to move + m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc); + return ControlState::DRIVE; + } + } else if (m_control_state == ControlState::EMERGENCY) { + if (stopped_condition && !emergency_condition) { + return ControlState::STOPPED; + } + } + + return current_control_state; +} + +PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( + const ControlState & current_control_state, const geometry_msgs::msg::Pose & current_pose, + const ControlData & control_data) +{ + const size_t nearest_idx = control_data.nearest_idx; + const float64_t current_vel = control_data.current_motion.vel; + const float64_t current_acc = control_data.current_motion.acc; + + // velocity and acceleration command + Motion raw_ctrl_cmd{}; + Motion target_motion{}; + if (current_control_state == ControlState::DRIVE) { + const auto target_pose = trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay( + current_pose, m_delay_compensation_time, current_vel); + const auto target_interpolated_point = + calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose.position, nearest_idx); + target_motion = Motion{ + target_interpolated_point.longitudinal_velocity_mps, + target_interpolated_point.acceleration_mps2}; + + target_motion = keepBrakeBeforeStop(*m_trajectory_ptr, target_motion, nearest_idx); + + const float64_t pred_vel_in_target = + predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); + m_debug_values.setValues( + trajectory_follower::DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); + + raw_ctrl_cmd.vel = target_motion.vel; + raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); + RCLCPP_DEBUG( + logger_, + "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " + "feedback_ctrl_cmd.ac: %3.3f", + raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, current_vel, target_motion.vel, + raw_ctrl_cmd.acc); + } else if (current_control_state == ControlState::STOPPING) { + raw_ctrl_cmd.acc = m_smooth_stop.calculate( + control_data.stop_dist, current_vel, current_acc, m_vel_hist, m_delay_compensation_time); + raw_ctrl_cmd.vel = m_stopped_state_params.vel; + + RCLCPP_DEBUG( + logger_, "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, + raw_ctrl_cmd.acc); + } else if (current_control_state == ControlState::STOPPED) { + // This acceleration is without slope compensation + const auto & p = m_stopped_state_params; + raw_ctrl_cmd.vel = p.vel; + raw_ctrl_cmd.acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); + + RCLCPP_DEBUG(logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); + } else if (current_control_state == ControlState::EMERGENCY) { + raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); + } + + // store acceleration without slope compensation + m_prev_raw_ctrl_cmd = raw_ctrl_cmd; + + // apply slope compensation and filter acceleration and jerk + const float64_t filtered_acc_cmd = calcFilteredAcc(raw_ctrl_cmd.acc, control_data); + const Motion filtered_ctrl_cmd{raw_ctrl_cmd.vel, filtered_acc_cmd}; + + // update debug visualization + updateDebugVelAcc(target_motion, current_pose, control_data); + + return filtered_ctrl_cmd; +} + +// Do not use nearest_idx here +autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController::createCtrlCmdMsg( + const Motion & ctrl_cmd, float64_t current_vel) +{ + // publish control command + autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; + cmd.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); + cmd.speed = static_cast(ctrl_cmd.vel); + cmd.acceleration = static_cast(ctrl_cmd.acc); + // m_pub_control_cmd->publish(cmd); + + // store current velocity history + m_vel_hist.push_back({rclcpp::Clock{RCL_ROS_TIME}.now(), current_vel}); + while (m_vel_hist.size() > static_cast(0.5 / m_longitudinal_ctrl_period)) { + m_vel_hist.erase(m_vel_hist.begin()); + } + + m_prev_ctrl_cmd = ctrl_cmd; + + return cmd; +} + +void PidLongitudinalController::publishDebugData( + const Motion & ctrl_cmd, const ControlData & control_data) +{ + using trajectory_follower::DebugValues; + // set debug values + m_debug_values.setValues(DebugValues::TYPE::DT, control_data.dt); + m_debug_values.setValues(DebugValues::TYPE::CALCULATED_ACC, control_data.current_motion.acc); + m_debug_values.setValues(DebugValues::TYPE::SHIFT, static_cast(control_data.shift)); + m_debug_values.setValues(DebugValues::TYPE::STOP_DIST, control_data.stop_dist); + m_debug_values.setValues( + DebugValues::TYPE::CONTROL_STATE, static_cast(m_control_state)); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PUBLISHED, ctrl_cmd.acc); + + // publish debug values + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic debug_msg{}; + debug_msg.diag_header.data_stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); + for (const auto & v : m_debug_values.getValues()) { + debug_msg.diag_array.data.push_back( + static_cast(v)); + } + m_pub_debug->publish(debug_msg); + + // slope angle + autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic slope_msg{}; + slope_msg.diag_header.data_stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); + slope_msg.diag_array.data.push_back( + static_cast(control_data.slope_angle)); + m_pub_slope->publish(slope_msg); +} + +float64_t PidLongitudinalController::getDt() +{ + float64_t dt; + if (!m_prev_control_time) { + dt = m_longitudinal_ctrl_period; + m_prev_control_time = std::make_shared(rclcpp::Clock{RCL_ROS_TIME}.now()); + } else { + dt = (rclcpp::Clock{RCL_ROS_TIME}.now() - *m_prev_control_time).seconds(); + *m_prev_control_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + } + const float64_t max_dt = m_longitudinal_ctrl_period * 2.0; + const float64_t min_dt = m_longitudinal_ctrl_period * 0.5; + return std::max(std::min(dt, max_dt), min_dt); +} + +PidLongitudinalController::Motion PidLongitudinalController::getCurrentMotion() const +{ + const float64_t dv = + m_current_velocity_ptr->twist.twist.linear.x - m_prev_velocity_ptr->twist.twist.linear.x; + const float64_t dt = std::max( + (rclcpp::Time(m_current_velocity_ptr->header.stamp) - + rclcpp::Time(m_prev_velocity_ptr->header.stamp)) + .seconds(), + 1e-03); + const float64_t accel = dv / dt; + + const float64_t current_vel = m_current_velocity_ptr->twist.twist.linear.x; + const float64_t current_acc = m_lpf_acc->filter(accel); + + return Motion{current_vel, current_acc}; +} + +enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift( + const size_t nearest_idx) const +{ + constexpr float64_t epsilon = 1e-5; + + const float64_t target_vel = m_trajectory_ptr->points.at(nearest_idx).longitudinal_velocity_mps; + + if (target_vel > epsilon) { + return Shift::Forward; + } else if (target_vel < -epsilon) { + return Shift::Reverse; + } + + return m_prev_shift; +} + +float64_t PidLongitudinalController::calcFilteredAcc( + const float64_t raw_acc, const ControlData & control_data) +{ + using trajectory_follower::DebugValues; + const float64_t acc_max_filtered = ::motion::motion_common::clamp(raw_acc, m_min_acc, m_max_acc); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, acc_max_filtered); + + // store ctrl cmd without slope filter + storeAccelCmd(acc_max_filtered); + + const float64_t acc_slope_filtered = + applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered); + + // This jerk filter must be applied after slope compensation + const float64_t acc_jerk_filtered = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered); + + return acc_jerk_filtered; +} + +void PidLongitudinalController::storeAccelCmd(const float64_t accel) +{ + if (m_control_state == ControlState::DRIVE) { + // convert format + autoware_auto_control_msgs::msg::LongitudinalCommand cmd; + cmd.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); + cmd.acceleration = static_cast(accel); + + // store published ctrl cmd + m_ctrl_cmd_vec.emplace_back(cmd); + } else { + // reset command + m_ctrl_cmd_vec.clear(); + } + + // remove unused ctrl cmd + if (m_ctrl_cmd_vec.size() <= 2) { + return; + } + if ( + (rclcpp::Clock{RCL_ROS_TIME}.now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > + m_delay_compensation_time) { + m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin()); + } +} + +float64_t PidLongitudinalController::applySlopeCompensation( + const float64_t input_acc, const float64_t pitch, const Shift shift) const +{ + if (!m_enable_slope_compensation) { + return input_acc; + } + const float64_t pitch_limited = std::min(std::max(pitch, m_min_pitch_rad), m_max_pitch_rad); + + // Acceleration command is always positive independent of direction (= shift) when car is running + float64_t sign = (shift == Shift::Forward) ? -1 : (shift == Shift::Reverse ? 1 : 0); + float64_t compensated_acc = input_acc + sign * 9.81 * std::sin(pitch_limited); + return compensated_acc; +} + +PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop( + const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, + const size_t nearest_idx) const +{ + Motion output_motion = target_motion; + + if (m_enable_brake_keeping_before_stop == false) { + return output_motion; + } + // const auto stop_idx = tier4_autoware_utils::searchZeroVelocityIndex(traj.points); + const auto stop_idx = motion_common::searchZeroVelocityIndex(traj.points); + if (!stop_idx) { + return output_motion; + } + + float64_t min_acc_before_stop = std::numeric_limits::max(); + size_t min_acc_idx = std::numeric_limits::max(); + for (int i = static_cast(*stop_idx); i >= 0; --i) { + const auto ui = static_cast(i); + if (traj.points.at(ui).acceleration_mps2 > static_cast(min_acc_before_stop)) { + break; + } + min_acc_before_stop = traj.points.at(ui).acceleration_mps2; + min_acc_idx = ui; + } + + const float64_t brake_keeping_acc = std::max(m_brake_keeping_acc, min_acc_before_stop); + if (nearest_idx >= min_acc_idx && target_motion.acc > brake_keeping_acc) { + output_motion.acc = brake_keeping_acc; + } + + return output_motion; +} + +autoware_auto_planning_msgs::msg::TrajectoryPoint +PidLongitudinalController::calcInterpolatedTargetValue( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const geometry_msgs::msg::Point & point, const size_t nearest_idx) const +{ + if (traj.points.size() == 1) { + return traj.points.at(0); + } + + // If the current position is not within the reference trajectory, enable the edge value. + // Else, apply linear interpolation + if (nearest_idx == 0) { + if (motion_common::calcSignedArcLength(traj.points, point, 0) > 0) { + return traj.points.at(0); + } + } + if (nearest_idx == traj.points.size() - 1) { + if (motion_common::calcSignedArcLength(traj.points, point, traj.points.size() - 1) < 0) { + return traj.points.at(traj.points.size() - 1); + } + } + + // apply linear interpolation + return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint(traj.points, point); +} + +float64_t PidLongitudinalController::predictedVelocityInTargetPoint( + const Motion current_motion, const float64_t delay_compensation_time) const +{ + const float64_t current_vel = current_motion.vel; + const float64_t current_acc = current_motion.acc; + + if (std::fabs(current_vel) < 1e-01) { // when velocity is low, no prediction + return current_vel; + } + + const float64_t current_vel_abs = std::fabs(current_vel); + if (m_ctrl_cmd_vec.size() == 0) { + const float64_t pred_vel = current_vel + current_acc * delay_compensation_time; + // avoid to change sign of current_vel and pred_vel + return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; + } + + float64_t pred_vel = current_vel_abs; + + const auto past_delay_time = + rclcpp::Clock{RCL_ROS_TIME}.now() - rclcpp::Duration::from_seconds(delay_compensation_time); + for (std::size_t i = 0; i < m_ctrl_cmd_vec.size(); ++i) { + if ( + (rclcpp::Clock{RCL_ROS_TIME}.now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < + m_delay_compensation_time) { + if (i == 0) { + // size of m_ctrl_cmd_vec is less than m_delay_compensation_time + pred_vel = current_vel_abs + static_cast(m_ctrl_cmd_vec.at(i).acceleration) * + delay_compensation_time; + return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; + } + // add velocity to accel * dt + const float64_t acc = m_ctrl_cmd_vec.at(i - 1).acceleration; + const auto curr_time_i = rclcpp::Time(m_ctrl_cmd_vec.at(i).stamp); + const float64_t time_to_next_acc = std::min( + (curr_time_i - rclcpp::Time(m_ctrl_cmd_vec.at(i - 1).stamp)).seconds(), + (curr_time_i - past_delay_time).seconds()); + pred_vel += acc * time_to_next_acc; + } + } + + const float64_t last_acc = m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).acceleration; + const float64_t time_to_current = + (rclcpp::Clock{RCL_ROS_TIME}.now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp) + .seconds(); + pred_vel += last_acc * time_to_current; + + // avoid to change sign of current_vel and pred_vel + return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; +} + +float64_t PidLongitudinalController::applyVelocityFeedback( + const Motion target_motion, const float64_t dt, const float64_t current_vel) +{ + using trajectory_follower::DebugValues; + const float64_t current_vel_abs = std::fabs(current_vel); + const float64_t target_vel_abs = std::fabs(target_motion.vel); + const bool8_t enable_integration = (current_vel_abs > m_current_vel_threshold_pid_integrate); + const float64_t error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs); + + std::vector pid_contributions(3); + const float64_t pid_acc = + m_pid_vel.calculate(error_vel_filtered, dt, enable_integration, pid_contributions); + const float64_t feedback_acc = target_motion.acc + pid_acc; + + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PID_APPLIED, feedback_acc); + m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL_FILTERED, error_vel_filtered); + m_debug_values.setValues( + DebugValues::TYPE::ACC_CMD_FB_P_CONTRIBUTION, pid_contributions.at(0)); // P + m_debug_values.setValues( + DebugValues::TYPE::ACC_CMD_FB_I_CONTRIBUTION, pid_contributions.at(1)); // I + m_debug_values.setValues( + DebugValues::TYPE::ACC_CMD_FB_D_CONTRIBUTION, pid_contributions.at(2)); // D + + return feedback_acc; +} + +void PidLongitudinalController::updatePitchDebugValues( + const float64_t pitch, const float64_t traj_pitch, const float64_t raw_pitch) +{ + using trajectory_follower::DebugValues; + const float64_t to_degrees = (180.0 / static_cast(autoware::common::types::PI)); + m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch); + m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, raw_pitch); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, raw_pitch * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_RAD, traj_pitch); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees); +} + +void PidLongitudinalController::updateDebugVelAcc( + const Motion & target_motion, const geometry_msgs::msg::Pose & current_pose, + const ControlData & control_data) +{ + using trajectory_follower::DebugValues; + const float64_t current_vel = control_data.current_motion.vel; + const size_t nearest_idx = control_data.nearest_idx; + + const auto interpolated_point = + calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose.position, nearest_idx); + + m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel); + m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel); + m_debug_values.setValues(DebugValues::TYPE::TARGET_ACC, target_motion.acc); + m_debug_values.setValues( + DebugValues::TYPE::NEAREST_VEL, interpolated_point.longitudinal_velocity_mps); + m_debug_values.setValues(DebugValues::TYPE::NEAREST_ACC, interpolated_point.acceleration_mps2); + m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL, target_motion.vel - current_vel); +} + +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt index 7996ac0713107..1275072cb86e4 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_nodes/CMakeLists.txt @@ -46,6 +46,18 @@ rclcpp_components_register_node(${LATLON_MUXER_NODE} EXECUTABLE ${LATLON_MUXER_NODE}_exe ) +set(CONTROLLER_NODE controller_node) +ament_auto_add_library(${CONTROLLER_NODE} SHARED +include/trajectory_follower_nodes/controller_node.hpp + src/controller_node.cpp +) + +target_compile_options(${CONTROLLER_NODE} PRIVATE -Wno-error=old-style-cast) +rclcpp_components_register_node(${CONTROLLER_NODE} + PLUGIN "autoware::motion::control::trajectory_follower_nodes::Controller" + EXECUTABLE ${CONTROLLER_NODE}_exe +) + if(BUILD_TESTING) set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_nodes) ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} @@ -57,10 +69,16 @@ if(BUILD_TESTING) ament_target_dependencies(${TRAJECTORY_FOLLOWER_NODES_TEST} fake_test_node) target_link_libraries(${TRAJECTORY_FOLLOWER_NODES_TEST} ${LATLON_MUXER_NODE} ${LATERAL_CONTROLLER_NODE} ${LONGITUDINAL_CONTROLLER_NODE}) - #find_package(autoware_testing REQUIRED) - #add_smoke_test(${PROJECT_NAME} ${LATLON_MUXER_NODE}_exe PARAM_FILENAME latlon_muxer_defaults.param.yaml) - #add_smoke_test(${PROJECT_NAME} ${LATERAL_CONTROLLER_NODE}_exe PARAM_FILENAME lateral_controller_defaults.param.yaml) - #add_smoke_test(${PROJECT_NAME} ${LONGITUDINAL_CONTROLLER_NODE}_exe PARAM_FILENAME longitudinal_controller_defaults.param.yaml) + find_package(autoware_testing REQUIRED) + add_smoke_test(${PROJECT_NAME} ${LATLON_MUXER_NODE}_exe + PARAM_FILENAMES "latlon_muxer_defaults.param.yaml" + ) + add_smoke_test(${PROJECT_NAME} ${LATERAL_CONTROLLER_NODE}_exe + PARAM_FILENAMES "lateral_controller_defaults.param.yaml test_vehicle_info.param.yaml" + ) + add_smoke_test(${PROJECT_NAME} ${LONGITUDINAL_CONTROLLER_NODE}_exe + PARAM_FILENAMES "longitudinal_controller_defaults.param.yaml test_vehicle_info.param.yaml" + ) endif() ament_auto_package( diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp new file mode 100644 index 0000000000000..73ead9b57266f --- /dev/null +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp @@ -0,0 +1,89 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 TRAJECTORY_FOLLOWER_NODES__CONTROLLER_NODE_HPP_ +#define TRAJECTORY_FOLLOWER_NODES__CONTROLLER_NODE_HPP_ + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Geometry" +#include "motion_common/motion_common.hpp" +#include "motion_common/trajectory_common.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "trajectory_follower/debug_values.hpp" +#include "trajectory_follower/lateral_controller_base.hpp" +#include "trajectory_follower/longitudinal_controller_base.hpp" +#include "trajectory_follower/longitudinal_controller_utils.hpp" +#include "trajectory_follower/lowpass_filter.hpp" +#include "trajectory_follower/pid.hpp" +#include "trajectory_follower/smooth_stop.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" +#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower_nodes +{ +using autoware::common::types::bool8_t; +using autoware::common::types::float64_t; +namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; +namespace motion_common = ::autoware::motion::motion_common; + +/// \classController +/// \brief The node class used for generating longitudinal control commands (velocity/acceleration) +class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node +{ +public: + explicit Controller(const rclcpp::NodeOptions & node_options); + +private: + // ros variables + rclcpp::TimerBase::SharedPtr m_timer_control_; + + std::shared_ptr longitudinal_controller_; + std::shared_ptr lateral_controller_; + + rclcpp::Publisher::SharedPtr + m_control_cmd_pub_; + + /** + * @brief compute control command, and publish periodically + */ + void callbackTimerControl(); +}; +} // namespace trajectory_follower_nodes +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER_NODES__CONTROLLER_NODE_HPP_ diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml index e692fa8e14811..911af5291e3c5 100644 --- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: # -- system -- - ctrl_period: 0.03 # control period [s] 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 @@ -56,6 +55,8 @@ # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 + converged_steer_rad: 0.1 + check_steer_converged_for_stopped_state: true # vehicle parameters mass_kg: 2400.0 diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml index 129abd2680509..ee625c99de083 100644 --- a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml @@ -1,11 +1,11 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 # control period [s] delay_compensation_time: 0.17 enable_smooth_stop: true enable_overshoot_emergency: true enable_slope_compensation: false + enable_keep_stopped_until_steer_convergence: true # state transition drive_state_stop_dist: 0.5 diff --git a/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml b/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml index b1279b50ef04a..8941b92b4e78e 100644 --- a/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml +++ b/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml @@ -9,3 +9,4 @@ left_overhang: 0.1 # between left wheel center and vehicle left right_overhang: 0.1 # between right wheel center and vehicle right vehicle_height: 2.5 + max_steer_angle: 0.70 # [rad] diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp new file mode 100644 index 0000000000000..6061074489b16 --- /dev/null +++ b/control/trajectory_follower_nodes/src/controller_node.cpp @@ -0,0 +1,80 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "trajectory_follower_nodes/controller_node.hpp" + +#include "motion_common/motion_common.hpp" +#include "motion_common/trajectory_common.hpp" +#include "time_utils/time_utils.hpp" +#include "trajectory_follower/mpc_lateral_controller.hpp" +#include "trajectory_follower/pid_longitudinal_controller.hpp" + +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower_nodes +{ +Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("controller", node_options) +{ + using std::placeholders::_1; + + const double m_ctrl_period = declare_parameter("ctrl_period", 0.015); + + lateral_controller_ = std::make_shared(this); + longitudinal_controller_ = std::make_shared(this); + + m_control_cmd_pub_ = create_publisher( + "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); + + // Timer + { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(m_ctrl_period)); + m_timer_control_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); + } +} + +void Controller::callbackTimerControl() +{ + const auto longitudinal_output = longitudinal_controller_->run(); + const auto lateral_output = lateral_controller_->run(); + + longitudinal_controller_->sync(lateral_output.sync_data); + lateral_controller_->sync(longitudinal_output.sync_data); + + autoware_auto_control_msgs::msg::AckermannControlCommand out; + out.stamp = this->now(); + out.lateral = lateral_output.control_cmd; + out.longitudinal = longitudinal_output.control_cmd; + m_control_cmd_pub_->publish(out); +} + +} // namespace trajectory_follower_nodes +} // namespace control +} // namespace motion +} // namespace autoware + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_nodes::Controller) diff --git a/control/vehicle_cmd_gate/CMakeLists.txt b/control/vehicle_cmd_gate/CMakeLists.txt index 886842aa7f634..309a296acc9db 100644 --- a/control/vehicle_cmd_gate/CMakeLists.txt +++ b/control/vehicle_cmd_gate/CMakeLists.txt @@ -15,7 +15,7 @@ rclcpp_components_register_node(vehicle_cmd_gate_node ) if(BUILD_TESTING) - ament_add_gtest(test_vehicle_cmd_gate + ament_add_ros_isolated_gtest(test_vehicle_cmd_gate test/src/test_main.cpp test/src/test_vehicle_cmd_filter.cpp ) diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index cb30c913ad766..7cb8b18244d18 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -26,7 +26,7 @@ tier4_vehicle_msgs vehicle_info_util - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index f8a129af4d2a4..75bd287291f2a 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -2,7 +2,6 @@ ros__parameters: # -- system -- - ctrl_period: 0.03 # control period [s] 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 @@ -57,6 +56,8 @@ # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 + converged_steer_rad: 0.1 + check_steer_converged_for_stopped_state: true # vehicle parameters vehicle: diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml index b3af131b2e778..31a5974333d67 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml @@ -1,11 +1,11 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 # control period [s] delay_compensation_time: 0.17 enable_smooth_stop: true enable_overshoot_emergency: true enable_slope_compensation: false + enable_keep_stopped_until_steer_convergence: true # state transition drive_state_stop_dist: 0.5 diff --git a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml index 4749bd45d3e3e..fd9d1ba6f3cda 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml @@ -9,6 +9,6 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index e8661322215fe..89d04bc5b3a00 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -92,20 +92,23 @@ def launch_setup(context, *args, **kwargs): surround_obstacle_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] surround_obstacle_checker_component = ComposableNode( package="surround_obstacle_checker", - plugin="SurroundObstacleCheckerNode", + plugin="surround_obstacle_checker::SurroundObstacleCheckerNode", name="surround_obstacle_checker", namespace="", remappings=[ ("~/output/no_start_reason", "/planning/scenario_planning/status/no_start_reason"), ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), - ("~/output/trajectory", "surround_obstacle_checker/trajectory"), + ("~/output/max_velocity", "/planning/scenario_planning/max_velocity_candidates"), + ( + "~/output/velocity_limit_clear_command", + "/planning/scenario_planning/clear_velocity_limit", + ), ( "~/input/pointcloud", "/perception/obstacle_segmentation/pointcloud", ), ("~/input/objects", "/perception/object_recognition/objects"), ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), ], parameters=[ surround_obstacle_checker_param, @@ -114,22 +117,6 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - # relay - relay_component = ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="skip_surround_obstacle_check_relay", - namespace="", - parameters=[ - { - "input_topic": "obstacle_avoidance_planner/trajectory", - "output_topic": "surround_obstacle_checker/trajectory", - "type": "autoware_auto_planning_msgs/msg/Trajectory", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - # obstacle stop planner obstacle_stop_planner_param_path = os.path.join( get_package_share_directory("tier4_planning_launch"), @@ -173,7 +160,7 @@ def launch_setup(context, *args, **kwargs): ), ("~/input/objects", "/perception/object_recognition/objects"), ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/trajectory", "surround_obstacle_checker/trajectory"), + ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), ], parameters=[ common_param, @@ -202,13 +189,7 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")), ) - relay_loader = LoadComposableNodes( - composable_node_descriptions=[relay_component], - target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_surround_obstacle_check")), - ) - - group = GroupAction([container, surround_obstacle_checker_loader, relay_loader]) + group = GroupAction([container, surround_obstacle_checker_loader]) return [group] diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 0fa8707e4fda9..3d195185d9bd6 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -19,22 +19,11 @@ - - - - - - - - - - - @@ -45,7 +34,7 @@ - + diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index fa894c8a7b1a5..3a3edd5338a58 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -13,8 +13,8 @@ ament_auto_add_executable(ekf_localizer ament_target_dependencies(ekf_localizer kalman_filter) # if(BUILD_TESTING) -# find_package(ament_cmake_gtest REQUIRED) -# ament_add_gtest(ekf_localizer-test test/test_ekf_localizer.test +# find_package(ament_cmake_ros REQUIRED) +# ament_add_ros_isolated_gtest(ekf_localizer-test test/test_ekf_localizer.test # test/src/test_ekf_localizer.cpp # src/ekf_localizer.cpp # src/kalman_filter/kalman_filter.cpp diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index c67cfaa400a0d..3e9d9735ae3bd 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -24,7 +24,7 @@ tier4_autoware_utils tier4_debug_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index b4b948c236af4..47fcf3282efea 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -17,7 +17,7 @@ tf2 tier4_debug_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/map/lanelet2_extension/CMakeLists.txt b/map/lanelet2_extension/CMakeLists.txt index d256029e4041a..709c7bdaa6858 100644 --- a/map/lanelet2_extension/CMakeLists.txt +++ b/map/lanelet2_extension/CMakeLists.txt @@ -62,15 +62,15 @@ target_link_libraries(autoware_lanelet2_validation ) if(BUILD_TESTING) - ament_add_gtest(message_conversion-test test/src/test_message_conversion.cpp) + ament_add_ros_isolated_gtest(message_conversion-test test/src/test_message_conversion.cpp) target_link_libraries(message_conversion-test lanelet2_extension_lib) - ament_add_gtest(projector-test test/src/test_projector.cpp) + ament_add_ros_isolated_gtest(projector-test test/src/test_projector.cpp) target_link_libraries(projector-test lanelet2_extension_lib) - ament_add_gtest(query-test test/src/test_query.cpp) + ament_add_ros_isolated_gtest(query-test test/src/test_query.cpp) target_link_libraries(query-test lanelet2_extension_lib) - ament_add_gtest(regulatory_elements-test test/src/test_regulatory_elements.cpp) + ament_add_ros_isolated_gtest(regulatory_elements-test test/src/test_regulatory_elements.cpp) target_link_libraries(regulatory_elements-test lanelet2_extension_lib) - ament_add_gtest(utilities-test test/src/test_utilities.cpp) + ament_add_ros_isolated_gtest(utilities-test test/src/test_utilities.cpp) target_link_libraries(utilities-test lanelet2_extension_lib) endif() diff --git a/map/lanelet2_extension/package.xml b/map/lanelet2_extension/package.xml index e817b06f1be86..d46d05a7f3064 100644 --- a/map/lanelet2_extension/package.xml +++ b/map/lanelet2_extension/package.xml @@ -27,7 +27,7 @@ tier4_autoware_utils visualization_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/mkdocs.yaml b/mkdocs.yaml index 2bd16349b24bb..02c3bb42383e8 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -66,7 +66,6 @@ markdown_extensions: - mdx_math - mdx_truly_sane_lists: nested_indent: 2 - - mdx_unimoji - plantuml_markdown: server: http://www.plantuml.com/plantuml format: svg diff --git a/perception/detected_object_feature_remover/package.xml b/perception/detected_object_feature_remover/package.xml index 686c01621c546..b609ff0257f1e 100644 --- a/perception/detected_object_feature_remover/package.xml +++ b/perception/detected_object_feature_remover/package.xml @@ -16,7 +16,7 @@ rclcpp_components tier4_perception_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp index ff790894658ec..9ecd336818b59 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp @@ -38,7 +38,8 @@ using sensor_msgs::msg::RegionOfInterest; class Debugger { public: - explicit Debugger(rclcpp::Node * node_ptr, const std::size_t image_num); + explicit Debugger( + rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size); void publishImage(const std::size_t image_id, const rclcpp::Time & stamp); @@ -57,7 +58,7 @@ class Debugger std::vector image_subs_; std::vector image_pubs_; std::vector> image_buffers_; - std::size_t image_buffer_size{5}; + std::size_t image_buffer_size_; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 8fe94963ff8ca..e5824dfcff97f 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -24,6 +24,7 @@ + @@ -70,6 +71,7 @@ + diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index f6466e6512a09..be981478e78a2 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -24,6 +24,7 @@ + @@ -70,6 +71,7 @@ + diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp index e1254fc136304..0bcfb019d93df 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -34,9 +34,12 @@ void drawRoiOnImage( namespace image_projection_based_fusion { -Debugger::Debugger(rclcpp::Node * node_ptr, const std::size_t image_num) : node_ptr_(node_ptr) +Debugger::Debugger( + rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size) +: node_ptr_(node_ptr) { image_buffers_.resize(image_num); + image_buffer_size_ = image_buffer_size; for (std::size_t img_i = 0; img_i < image_num; ++img_i) { auto sub = image_transport::create_subscription( node_ptr, "input/image_raw" + std::to_string(img_i), @@ -54,7 +57,7 @@ Debugger::Debugger(rclcpp::Node * node_ptr, const std::size_t image_num) : node_ auto pub = image_transport::create_publisher(node_ptr, "output/image_raw" + std::to_string(img_i)); image_pubs_.push_back(pub); - image_buffers_.at(img_i).set_capacity(image_buffer_size); + image_buffers_.at(img_i).set_capacity(image_buffer_size_); } } diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 57ed245f67bd7..5d33303ca1bf9 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -127,7 +127,9 @@ FusionNode::FusionNode(const std::string & node_name, const rclcpp::NodeOpt // debugger if (declare_parameter("debug_mode", false)) { - debugger_ = std::make_shared(this, rois_number_); + std::size_t image_buffer_size = + static_cast(declare_parameter("image_buffer_size", 15)); + debugger_ = std::make_shared(this, rois_number_, image_buffer_size); } } diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt index 4fee2ecdd2f3b..433999051097d 100644 --- a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt @@ -4,9 +4,6 @@ project(lidar_apollo_instance_segmentation) find_package(autoware_cmake REQUIRED) autoware_package() -# TODO: remove (https://github.com/autowarefoundation/autoware.universe/issues/850) -add_compile_options(-Wno-deprecated-declarations) - option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) # set flags for CUDA availability diff --git a/perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp b/perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp index 30d560135cb4f..14c1ff6d44c63 100644 --- a/perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp +++ b/perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp @@ -56,13 +56,13 @@ class trtNet } if (!mTrtRunTime) { - mTrtRunTime->destroy(); + delete mTrtRunTime; } if (!mTrtContext) { - mTrtContext->destroy(); + delete mTrtContext; } if (!mTrtEngine) { - mTrtEngine->destroy(); + delete mTrtEngine; } } diff --git a/perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp b/perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp index c51adc83e07c1..0f1da4bdac8a6 100644 --- a/perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp +++ b/perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp @@ -100,7 +100,7 @@ trtNet::trtNet(const std::string & engineFile) mTrtRunTime = createInferRuntime(gLogger); assert(mTrtRunTime != nullptr); - mTrtEngine = mTrtRunTime->deserializeCudaEngine(data.get(), length, nullptr); + mTrtEngine = mTrtRunTime->deserializeCudaEngine(data.get(), length); assert(mTrtEngine != nullptr); InitEngine(); diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index 7d628173b2e4c..af8b29f55f340 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -63,17 +63,27 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * const int batch_size = 1; builder->setMaxBatchSize(batch_size); config->setMaxWorkspaceSize(1 << 30); - nvinfer1::ICudaEngine * engine = builder->buildEngineWithConfig(*network, *config); - nvinfer1::IHostMemory * trt_model_stream = engine->serialize(); - assert(trt_model_stream != nullptr); + nvinfer1::IHostMemory * plan = builder->buildSerializedNetwork(*network, *config); + assert(plan != nullptr); std::ofstream outfile(engine_file, std::ofstream::binary); assert(!outfile.fail()); - outfile.write(reinterpret_cast(trt_model_stream->data()), trt_model_stream->size()); + outfile.write(reinterpret_cast(plan->data()), plan->size()); outfile.close(); - network->destroy(); - parser->destroy(); - builder->destroy(); - config->destroy(); + if (network) { + delete network; + } + if (parser) { + delete parser; + } + if (builder) { + delete builder; + } + if (config) { + delete config; + } + if (plan) { + delete plan; + } } net_ptr_.reset(new Tn::trtNet(engine_file)); diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 1f5c869136eb0..4f15575181348 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -4,9 +4,6 @@ project(lidar_centerpoint) find_package(autoware_cmake REQUIRED) autoware_package() -# TODO: remove (https://github.com/autowarefoundation/autoware.universe/issues/850) -add_compile_options(-Wno-deprecated-declarations -Wno-unknown-pragmas) - option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) # set flags for CUDA availability @@ -89,13 +86,13 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) message(STATUS "File already exists.") else() message(STATUS "File hash changes. Downloading now ...") - execute_process(COMMAND gdown --quiet https://drive.google.com//uc?id=${GFILE_ID} -O ${FILE_PATH}) + execute_process(COMMAND gdown --quiet https://drive.google.com/uc?id=${GFILE_ID} -O ${FILE_PATH}) # file(MD5 ${FILE_PATH} DOWNLOADED_FILE_HASH) # disable to pass ci message(STATUS "Downloaded file hash: ${DOWNLOADED_FILE_HASH}") endif() else() message(STATUS "File doesn't exists. Downloading now ...") - execute_process(COMMAND gdown --quiet https://drive.google.com//uc?id=${GFILE_ID} -O ${FILE_PATH}) + execute_process(COMMAND gdown --quiet https://drive.google.com/uc?id=${GFILE_ID} -O ${FILE_PATH}) # file(MD5 ${FILE_PATH} DOWNLOADED_FILE_HASH) # disable to pass ci message(STATUS "Downloaded file hash: ${DOWNLOADED_FILE_HASH}") endif() @@ -144,6 +141,13 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) centerpoint_cuda_libraries ) + # To suppress unknown-pragmas error. The root-cause is CUB library in CUDA 11.6. + # This issue was fixed by https://github.com/NVIDIA/cub/commit/7d608bf1dc14553e2fb219eabeed80b76621b6fe + target_include_directories(centerpoint + SYSTEM PUBLIC + ${CUDA_INCLUDE_DIRS} + ) + ## node ## ament_auto_add_library(lidar_centerpoint_component SHARED src/node.cpp diff --git a/perception/lidar_centerpoint/lib/include/tensorrt_wrapper.hpp b/perception/lidar_centerpoint/lib/include/tensorrt_wrapper.hpp index dd49723a5265c..b334be9e2230b 100644 --- a/perception/lidar_centerpoint/lib/include/tensorrt_wrapper.hpp +++ b/perception/lidar_centerpoint/lib/include/tensorrt_wrapper.hpp @@ -29,7 +29,7 @@ struct Deleter void operator()(T * obj) const { if (obj) { - obj->destroy(); + delete obj; } } }; @@ -82,6 +82,7 @@ class TensorRTWrapper bool createContext(); unique_ptr runtime_ = nullptr; + unique_ptr plan_ = nullptr; unique_ptr engine_ = nullptr; }; diff --git a/perception/lidar_centerpoint/lib/src/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/src/tensorrt_wrapper.cpp index 3691128137c39..f9882afac8f92 100644 --- a/perception/lidar_centerpoint/lib/src/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/src/tensorrt_wrapper.cpp @@ -104,7 +104,13 @@ bool TensorRTWrapper::parseONNX( std::cout << "Applying optimizations and building TRT CUDA engine (" << onnx_path << ") ..." << std::endl; - engine_ = unique_ptr(builder->buildEngineWithConfig(*network, *config)); + plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); + if (!plan_) { + std::cout << "Fail to create serialized network" << std::endl; + return false; + } + engine_ = unique_ptr( + runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { std::cout << "Fail to create engine" << std::endl; return false; @@ -116,9 +122,8 @@ bool TensorRTWrapper::parseONNX( bool TensorRTWrapper::saveEngine(const std::string & engine_path) { std::cout << "Writing to " << engine_path << std::endl; - auto serialized = unique_ptr(engine_->serialize()); std::ofstream file(engine_path, std::ios::out | std::ios::binary); - file.write(reinterpret_cast(serialized->data()), serialized->size()); + file.write(reinterpret_cast(plan_->data()), plan_->size()); return true; } @@ -139,8 +144,7 @@ bool TensorRTWrapper::loadEngine(const std::string & engine_path) } std::cout << "Loading from " << engine_path << std::endl; - engine_ = - unique_ptr(runtime_->deserializeCudaEngine(buffer.get(), size, nullptr)); + engine_ = unique_ptr(runtime_->deserializeCudaEngine(buffer.get(), size)); return true; } diff --git a/perception/tensorrt_yolo/CMakeLists.txt b/perception/tensorrt_yolo/CMakeLists.txt index c05fefc38d15c..e8ac60c30ed37 100755 --- a/perception/tensorrt_yolo/CMakeLists.txt +++ b/perception/tensorrt_yolo/CMakeLists.txt @@ -4,9 +4,6 @@ project(tensorrt_yolo) find_package(autoware_cmake REQUIRED) autoware_package() -# TODO: remove (https://github.com/autowarefoundation/autoware.universe/issues/850) -add_compile_options(-Wno-deprecated-declarations) - option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) find_package(OpenCV REQUIRED) @@ -96,7 +93,7 @@ message(STATUS "Checking and downloading yolov4.onnx") if(NOT EXISTS "${FILE}") message(STATUS "... file does not exist. Downloading now ...") execute_process( - COMMAND gdown "https://drive.google.com//uc?id=1vkNmSwcIpTkJ_-BrKhxtit0PBJeJYTVX" -O ${PATH}/yolov4.onnx + COMMAND gdown "https://drive.google.com/uc?id=1vkNmSwcIpTkJ_-BrKhxtit0PBJeJYTVX" -O ${PATH}/yolov4.onnx ERROR_QUIET ) endif() @@ -118,7 +115,7 @@ message(STATUS "Checking and downloading yolov5s.onnx") if(NOT EXISTS "${FILE}") message(STATUS "... file does not exist. Downloading now ...") execute_process( - COMMAND gdown "https://drive.google.com//uc?id=1CF21nQWigwCPTr5psQZXg6cBQIOYKbad" -O ${PATH}/yolov5s.onnx + COMMAND gdown "https://drive.google.com/uc?id=1CF21nQWigwCPTr5psQZXg6cBQIOYKbad" -O ${PATH}/yolov5s.onnx ERROR_QUIET ) endif() @@ -129,7 +126,7 @@ message(STATUS "Checking and downloading yolov5m.onnx") if(NOT EXISTS "${FILE}") message(STATUS "... file does not exist. Downloading now ...") execute_process( - COMMAND gdown "https://drive.google.com//uc?id=1a1h50KJH6slwmjKZpPlS-errukF-BrgG" -O ${PATH}/yolov5m.onnx + COMMAND gdown "https://drive.google.com/uc?id=1a1h50KJH6slwmjKZpPlS-errukF-BrgG" -O ${PATH}/yolov5m.onnx ERROR_QUIET ) endif() diff --git a/perception/tensorrt_yolo/README.md b/perception/tensorrt_yolo/README.md index 5dfae272f143a..5811fa88a6b7a 100644 --- a/perception/tensorrt_yolo/README.md +++ b/perception/tensorrt_yolo/README.md @@ -72,15 +72,15 @@ This package includes multiple licenses. ### YOLOv4 -[YOLOv4](https://drive.google.com//uc?id=1vkNmSwcIpTkJ_-BrKhxtit0PBJeJYTVX "YOLOv4"): Converted from darknet [weight file](https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v3_optimal/yolov4.weights "weight file") and [conf file](https://github.com/AlexeyAB/darknet/blob/master/cfg/yolov4.cfg "conf file"). +[YOLOv4](https://drive.google.com/uc?id=1vkNmSwcIpTkJ_-BrKhxtit0PBJeJYTVX "YOLOv4"): Converted from darknet [weight file](https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v3_optimal/yolov4.weights "weight file") and [conf file](https://github.com/AlexeyAB/darknet/blob/master/cfg/yolov4.cfg "conf file"). ### YOLOv5 Refer to [this guide](https://github.com/ultralytics/yolov5/issues/251 "guide") -- [YOLOv5s](https://drive.google.com//uc?id=1CF21nQWigwCPTr5psQZXg6cBQIOYKbad "YOLOv5s") +- [YOLOv5s](https://drive.google.com/uc?id=1CF21nQWigwCPTr5psQZXg6cBQIOYKbad "YOLOv5s") -- [YOLOv5m](https://drive.google.com//uc?id=1a1h50KJH6slwmjKZpPlS-errukF-BrgG "YOLOv5m") +- [YOLOv5m](https://drive.google.com/uc?id=1a1h50KJH6slwmjKZpPlS-errukF-BrgG "YOLOv5m") - [YOLOv5l](https://drive.google.com/uc?id=1xO8S92Cq7qrmx93UHHyA7Cd7-dJsBDP8 "YOLOv5l") diff --git a/perception/tensorrt_yolo/lib/include/trt_yolo.hpp b/perception/tensorrt_yolo/lib/include/trt_yolo.hpp index 813811226a0a6..c951154820f1a 100644 --- a/perception/tensorrt_yolo/lib/include/trt_yolo.hpp +++ b/perception/tensorrt_yolo/lib/include/trt_yolo.hpp @@ -59,7 +59,7 @@ struct Deleter void operator()(T * obj) const { if (obj) { - obj->destroy(); + delete obj; } } }; @@ -130,6 +130,7 @@ class Net private: unique_ptr runtime_ = nullptr; + unique_ptr plan_ = nullptr; unique_ptr engine_ = nullptr; unique_ptr context_ = nullptr; cudaStream_t stream_ = nullptr; diff --git a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp index 1be5f279714ba..748680266ca9c 100644 --- a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp +++ b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp @@ -65,8 +65,7 @@ void Net::load(const std::string & path) file.read(buffer, size); file.close(); if (runtime_) { - engine_ = - unique_ptr(runtime_->deserializeCudaEngine(buffer, size, nullptr)); + engine_ = unique_ptr(runtime_->deserializeCudaEngine(buffer, size)); } delete[] buffer; } @@ -253,9 +252,15 @@ Net::Net( // Build engine std::cout << "Applying optimizations and building TRT CUDA engine..." << std::endl; - engine_ = unique_ptr(builder->buildEngineWithConfig(*network, *config)); + plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); + if (!plan_) { + std::cout << "Fail to create serialized network" << std::endl; + return; + } + engine_ = unique_ptr( + runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!prepare()) { - std::cout << "Fail to prepare engine" << std::endl; + std::cout << "Fail to create engine" << std::endl; return; } } @@ -263,9 +268,8 @@ Net::Net( void Net::save(const std::string & path) const { std::cout << "Writing to " << path << "..." << std::endl; - auto serialized = unique_ptr(engine_->serialize()); std::ofstream file(path, std::ios::out | std::ios::binary); - file.write(reinterpret_cast(serialized->data()), serialized->size()); + file.write(reinterpret_cast(plan_->data()), plan_->size()); } void Net::infer(std::vector & buffers, const int batch_size) diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt index 08897bfa3cf5a..8ebd5f32db99e 100644 --- a/perception/traffic_light_classifier/CMakeLists.txt +++ b/perception/traffic_light_classifier/CMakeLists.txt @@ -4,9 +4,6 @@ project(traffic_light_classifier) find_package(autoware_cmake REQUIRED) autoware_package() -# TODO: remove (https://github.com/autowarefoundation/autoware.universe/issues/850) -add_compile_options(-Wno-deprecated-declarations) - option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) # set flags for CUDA availability option(CUDA_AVAIL "CUDA available" OFF) diff --git a/perception/traffic_light_classifier/utils/trt_common.cpp b/perception/traffic_light_classifier/utils/trt_common.cpp index a920d47fa295a..cae9f0cd1682a 100644 --- a/perception/traffic_light_classifier/utils/trt_common.cpp +++ b/perception/traffic_light_classifier/utils/trt_common.cpp @@ -45,6 +45,7 @@ TrtCommon::TrtCommon(std::string model_path, std::string precision) is_initialized_(false), max_batch_size_(1) { + runtime_ = UniquePtr(nvinfer1::createInferRuntime(logger_)); } void TrtCommon::setup() @@ -86,9 +87,8 @@ bool TrtCommon::loadEngine(std::string engine_file_path) std::stringstream engine_buffer; engine_buffer << engine_file.rdbuf(); std::string engine_str = engine_buffer.str(); - runtime_ = UniquePtr(nvinfer1::createInferRuntime(logger_)); engine_ = UniquePtr(runtime_->deserializeCudaEngine( - reinterpret_cast(engine_str.data()), engine_str.size(), nullptr)); + reinterpret_cast(engine_str.data()), engine_str.size())); return true; } @@ -117,19 +117,23 @@ bool TrtCommon::buildEngineFromOnnx(std::string onnx_file_path, std::string outp return false; } - engine_ = UniquePtr(builder->buildEngineWithConfig(*network, *config)); + auto plan = UniquePtr(builder->buildSerializedNetwork(*network, *config)); + if (!plan) { + return false; + } + engine_ = + UniquePtr(runtime_->deserializeCudaEngine(plan->data(), plan->size())); if (!engine_) { return false; } // save engine - nvinfer1::IHostMemory * data = engine_->serialize(); std::ofstream file; file.open(output_engine_file_path, std::ios::binary | std::ios::out); if (!file.is_open()) { return false; } - file.write((const char *)data->data(), data->size()); + file.write((const char *)plan->data(), plan->size()); file.close(); return true; diff --git a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt index e82c790f4fd1e..eb172b7faf981 100644 --- a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt @@ -4,9 +4,6 @@ project(traffic_light_ssd_fine_detector) find_package(autoware_cmake REQUIRED) autoware_package() -# TODO: remove (https://github.com/autowarefoundation/autoware.universe/issues/850) -add_compile_options(-Wno-deprecated-declarations) - option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) find_package(OpenCV REQUIRED) diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp index bbe5ae8da513c..cfb6c8a6d5754 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp +++ b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp @@ -31,7 +31,7 @@ struct Deleter void operator()(T * obj) const { if (obj) { - obj->destroy(); + delete obj; } } }; @@ -87,6 +87,7 @@ class Net private: unique_ptr runtime_ = nullptr; + unique_ptr plan_ = nullptr; unique_ptr engine_ = nullptr; unique_ptr context_ = nullptr; cudaStream_t stream_ = nullptr; diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp index 766de7eb4cde1..ba9f94b014513 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp +++ b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp @@ -36,8 +36,7 @@ void Net::load(const std::string & path) file.read(buffer, size); file.close(); if (runtime_) { - engine_ = - unique_ptr(runtime_->deserializeCudaEngine(buffer, size, nullptr)); + engine_ = unique_ptr(runtime_->deserializeCudaEngine(buffer, size)); } delete[] buffer; } @@ -136,7 +135,13 @@ Net::Net( // Build engine std::cout << "Applying optimizations and building TRT CUDA engine..." << std::endl; - engine_ = unique_ptr(builder->buildEngineWithConfig(*network, *config)); + plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); + if (!plan_) { + std::cout << "Fail to create serialized network" << std::endl; + return; + } + engine_ = unique_ptr( + runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { std::cout << "Fail to create engine" << std::endl; return; @@ -151,9 +156,8 @@ Net::Net( void Net::save(const std::string & path) { std::cout << "Writing to " << path << "..." << std::endl; - auto serialized = unique_ptr(engine_->serialize()); std::ofstream file(path, std::ios::out | std::ios::binary); - file.write(reinterpret_cast(serialized->data()), serialized->size()); + file.write(reinterpret_cast(plan_->data()), plan_->size()); } void Net::infer(std::vector & buffers, const int batch_size) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp index edfb7d7f84d71..d3f8f80606784 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp @@ -93,7 +93,7 @@ MarkerArray createPoseMarkerArray( MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects); -MarkerArray createOvehangFurthestLineStringMarkerArray( +MarkerArray createOverhangFurthestLineStringMarkerArray( const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r, const double g, const double b); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index b03282c735ece..2923996ca8697 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -490,24 +490,19 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( avoidance_debug_msg.max_shift_length = max_allowable_lateral_distance; if (!(o.to_road_shoulder_distance > max_allowable_lateral_distance)) { - avoidance_debug_msg.allow_avoidance = false; - avoidance_debug_msg.failed_reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; + avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN); + continue; } const auto max_shift_length = o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width; - const auto max_left_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length, - this]() noexcept { - const auto left_shift_constraint = std::min(getLeftShiftBound(), max_shift_length); - return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? left_shift_constraint - : 0.0; + + const auto max_left_shift_limit = [&max_shift_length, this]() noexcept { + return std::min(getLeftShiftBound(), max_shift_length); }; - const auto max_right_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length, - this]() noexcept { - const auto right_shift_constraint = std::max(getRightShiftBound(), -max_shift_length); - return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? right_shift_constraint - : 0.0; + const auto max_right_shift_limit = [&max_shift_length, this]() noexcept { + return std::max(getRightShiftBound(), -max_shift_length); }; const auto shift_length = isOnRight(o) @@ -1939,8 +1934,8 @@ void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(Shifted std::sqrt(v0 * v0 + 2.0 * s * parameters_.max_avoidance_acceleration)); // apply velocity limit - constexpr size_t VLIM_APPLY_IDX_MARGIN = 0; - for (size_t i = ego_idx + VLIM_APPLY_IDX_MARGIN; i < N; ++i) { + constexpr size_t V_LIM_APPLY_IDX_MARGIN = 0; + for (size_t i = ego_idx + V_LIM_APPLY_IDX_MARGIN; i < N; ++i) { path.path.points.at(i).point.longitudinal_velocity_mps = std::min(path.path.points.at(i).point.longitudinal_velocity_mps, static_cast(vmax)); } @@ -2619,7 +2614,7 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData using marker_utils::createAvoidPointMarkerArray; using marker_utils::createLaneletsAreaMarkerArray; using marker_utils::createObjectsMarkerArray; - using marker_utils::createOvehangFurthestLineStringMarkerArray; + using marker_utils::createOverhangFurthestLineStringMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; using marker_utils::createShiftLengthMarkerArray; @@ -2652,7 +2647,7 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0)); add(createAvoidanceObjectsMarkerArray(avoidance_data_.objects, "avoidance_object")); add(makeOverhangToRoadShoulderMarkerArray(avoidance_data_.objects)); - add(createOvehangFurthestLineStringMarkerArray( + add(createOverhangFurthestLineStringMarkerArray( *debug.farthest_linestring_from_overhang, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0)); // parent object info diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp index d7c3b1325f7dc..88c8d102bd00c 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp @@ -512,7 +512,7 @@ MarkerArray makeOverhangToRoadShoulderMarkerArray( return msg; } -MarkerArray createOvehangFurthestLineStringMarkerArray( +MarkerArray createOverhangFurthestLineStringMarkerArray( const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r, const double g, const double b) { diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 6acaa54b6481b..6e9671c166518 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -11,312 +11,70 @@ find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(OpenCV REQUIRED) -set(BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES - tier4_api_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - tier4_planning_msgs - tier4_autoware_utils - Boost - Eigen3 - diagnostic_msgs - geometry_msgs - lanelet2_extension - PCL - pcl_conversions - rclcpp - sensor_msgs - interpolation - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - vehicle_info_util - visualization_msgs - nav_msgs - grid_map_ros -) +set(scene_modules + detection_area + blind_spot + stop_line + crosswalk + traffic_light + intersection + no_stopping_area + virtual_traffic_light + occlusion_spot +) + +foreach(scene_module IN LISTS scene_modules) + file(GLOB_RECURSE scene_module_src "src/scene_module/${scene_module}/*") + list(APPEND scene_modules_src ${scene_module_src}) +endforeach() - -# Common -ament_auto_add_library(scene_module_lib SHARED +ament_auto_add_library(behavior_velocity_planner SHARED + src/node.cpp + src/planner_manager.cpp src/utilization/path_utilization.cpp src/utilization/util.cpp + ${scene_modules_src} ) -target_include_directories(scene_module_lib +target_include_directories(behavior_velocity_planner SYSTEM PUBLIC ${BOOST_INCLUDE_DIRS} - ${tf2_geometry_msgs_INCLUDE_DIRS} -) - -target_include_directories(scene_module_lib - PUBLIC - $ - $ -) - -target_include_directories(scene_module_lib - SYSTEM PUBLIC ${PCL_INCLUDE_DIRS} -) - -ament_target_dependencies(scene_module_lib ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -# Scene Module: Stop Line -ament_auto_add_library(scene_module_stop_line SHARED - src/scene_module/stop_line/manager.cpp - src/scene_module/stop_line/scene.cpp - src/scene_module/stop_line/debug.cpp -) - -target_include_directories(scene_module_stop_line - PUBLIC - $ - $ -) - -ament_target_dependencies(scene_module_stop_line ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(scene_module_stop_line scene_module_lib) - -# Scene Module: Crosswalk -ament_auto_add_library(scene_module_crosswalk SHARED - src/scene_module/crosswalk/manager.cpp - src/scene_module/crosswalk/scene_crosswalk.cpp - src/scene_module/crosswalk/scene_walkway.cpp - src/scene_module/crosswalk/debug.cpp - src/scene_module/crosswalk/util.cpp -) - -target_include_directories(scene_module_crosswalk - PUBLIC - $ - $ -) - -ament_target_dependencies(scene_module_crosswalk ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(scene_module_crosswalk scene_module_lib) - -# Scene Module: Intersection -ament_auto_add_library(scene_module_intersection SHARED - src/scene_module/intersection/manager.cpp - src/scene_module/intersection/scene_intersection.cpp - src/scene_module/intersection/scene_merge_from_private_road.cpp - src/scene_module/intersection/debug.cpp - src/scene_module/intersection/util.cpp -) - -target_include_directories(scene_module_intersection - PUBLIC - $ - $ -) - -ament_target_dependencies(scene_module_intersection ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(scene_module_intersection scene_module_lib) - -# Scene Module: Traffic Light -ament_auto_add_library(scene_module_traffic_light SHARED - src/scene_module/traffic_light/manager.cpp - src/scene_module/traffic_light/scene.cpp - src/scene_module/traffic_light/debug.cpp -) - -target_include_directories(scene_module_traffic_light - PUBLIC - $ - $ -) - -ament_target_dependencies(scene_module_traffic_light ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(scene_module_traffic_light scene_module_lib) - -# Scene Module: Blind Spot -ament_auto_add_library(scene_module_blind_spot SHARED - src/scene_module/blind_spot/manager.cpp - src/scene_module/blind_spot/scene.cpp - src/scene_module/blind_spot/debug.cpp -) - -target_include_directories(scene_module_blind_spot - PUBLIC - $ - $ -) - -ament_target_dependencies(scene_module_blind_spot ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(scene_module_blind_spot scene_module_lib) - -# Scene Module: Detection Area -ament_auto_add_library(scene_module_detection_area SHARED - src/scene_module/detection_area/manager.cpp - src/scene_module/detection_area/scene.cpp - src/scene_module/detection_area/debug.cpp -) - -target_include_directories(scene_module_detection_area - PUBLIC - $ - $ -) - -ament_target_dependencies(scene_module_detection_area ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(scene_module_detection_area scene_module_lib) - -# Scene Module: No Stopping Area -ament_auto_add_library(scene_module_no_stopping_area SHARED - src/scene_module/no_stopping_area/manager.cpp - src/scene_module/no_stopping_area/scene_no_stopping_area.cpp - src/scene_module/no_stopping_area/debug.cpp -) - -target_include_directories(scene_module_no_stopping_area - PUBLIC - $ - $ -) - -ament_target_dependencies(scene_module_no_stopping_area ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(scene_module_no_stopping_area scene_module_lib) - -# Scene Module: Virtual Traffic Light -ament_auto_add_library(scene_module_virtual_traffic_light SHARED - src/scene_module/virtual_traffic_light/manager.cpp - src/scene_module/virtual_traffic_light/scene.cpp - src/scene_module/virtual_traffic_light/debug.cpp -) - -target_include_directories(scene_module_virtual_traffic_light - PUBLIC - $ - $ -) - -ament_target_dependencies(scene_module_virtual_traffic_light ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(scene_module_virtual_traffic_light scene_module_lib) - -# SceneModule OcclusionSpot -# Util -ament_auto_add_library(occlusion_spot_lib SHARED - src/scene_module/occlusion_spot/grid_utils.cpp - src/scene_module/occlusion_spot/occlusion_spot_utils.cpp - src/scene_module/occlusion_spot/risk_predictive_braking.cpp -) - -target_include_directories(occlusion_spot_lib - SYSTEM PUBLIC - ${BOOST_INCLUDE_DIRS} ${tf2_geometry_msgs_INCLUDE_DIRS} ) -target_include_directories(occlusion_spot_lib - PUBLIC - $ - $ -) - -ament_target_dependencies(occlusion_spot_lib ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -ament_auto_add_library(scene_module_occlusion_spot SHARED - src/scene_module/occlusion_spot/manager.cpp - src/scene_module/occlusion_spot/debug.cpp - src/scene_module/occlusion_spot/scene_occlusion_spot.cpp -) - -target_include_directories(scene_module_occlusion_spot - PUBLIC - $ - $ -) - -ament_target_dependencies(scene_module_occlusion_spot ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(scene_module_occlusion_spot scene_module_lib occlusion_spot_lib) - -# Scene Module Manager -ament_auto_add_library(scene_module_manager SHARED - src/planner_manager.cpp -) - -target_include_directories(scene_module_manager - PUBLIC - $ - $ -) - -ament_target_dependencies(scene_module_manager ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(scene_module_manager scene_module_lib) - -# Node -ament_auto_add_library(behavior_velocity_planner SHARED - src/node.cpp -) - -target_include_directories(behavior_velocity_planner - PUBLIC - $ - $ -) - -ament_target_dependencies(behavior_velocity_planner ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - -target_link_libraries(behavior_velocity_planner - scene_module_lib - scene_module_stop_line - scene_module_crosswalk - scene_module_intersection - scene_module_traffic_light - scene_module_blind_spot - scene_module_occlusion_spot - scene_module_detection_area - scene_module_no_stopping_area - scene_module_virtual_traffic_light - scene_module_manager +ament_target_dependencies(behavior_velocity_planner + Boost + Eigen3 + PCL ) -ament_export_dependencies(${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) - rclcpp_components_register_node(behavior_velocity_planner PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" EXECUTABLE behavior_velocity_planner_node ) - if(BUILD_TESTING) - # utils for test - ament_auto_add_library(utilization SHARED - src/utilization/util.cpp - ) - # Gtest for utilization - ament_add_gtest(utilization-test + ament_add_ros_isolated_gtest(utilization-test test/src/test_state_machine.cpp test/src/test_arc_lane_util.cpp test/src/test_utilization.cpp ) target_link_libraries(utilization-test gtest_main - utilization + behavior_velocity_planner ) # Gtest for occlusion spot - ament_add_gtest(occlusion_spot-test + ament_add_ros_isolated_gtest(occlusion_spot-test test/src/test_occlusion_spot_utils.cpp test/src/test_risk_predictive_braking.cpp test/src/test_grid_utils.cpp ) target_link_libraries(occlusion_spot-test gtest_main - scene_module_occlusion_spot + behavior_velocity_planner ) endif() diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index a7feea30a3017..20ad38140d330 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -5,12 +5,15 @@ `behavior_velocity_planner` is a planner that adjust velocity based on the traffic rules. It consists of several modules. Please refer to the links listed below for detail on each module. +![Architecture](./docs/BehaviorVelocityPlanner-Architecture.drawio.svg) + - [Blind Spot](blind-spot-design.md) - [Crosswalk](crosswalk-design.md) - [Detection Area](detection-area-design.md) - [Intersection](intersection-design.md) - [MergeFromPrivate](merge-from-private-design.md) - [Stop Line](stop-line-design.md) +- [Virtual Traffic Light](virtual-traffic-light-design.md) - [Traffic Light](traffic-light-design.md) - [Occlusion Spot](occlusion-spot-design.md) - [No Stopping Area](no-stopping-area-design.md) diff --git a/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg b/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg new file mode 100644 index 0000000000000..4d2808a5be41a --- /dev/null +++ b/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg @@ -0,0 +1,1663 @@ + + + + + + + + +
+
+
+ /perception/occupancy_grid_map/map +
+
+
+ + + /per... + + + + + + + + + Blind Spot + + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + + updateSceneModuleInstance + + + + + + + + +
+
+
+ + add/delete modules + +
+
+
+ + + add/delete modules + + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + + modifyPathVelocity + + + + + + + +
+
+
+ + scene_modules + +
+
+
+ + + scene_modules + + + + + + + +
+
+
+ Turn RIght +
+
+
+ + + Turn RIght + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + +
+
+
+ Turn Left +
+
+
+ + + Turn Left + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + Occlusion Spot + + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + + updateSceneModuleInstance + + + + + + + + +
+
+
+ + add/delete modules + +
+
+
+ + + add/delete modules + + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + + modifyPathVelocity + + + + + + + +
+
+
+ + scene_modules + +
+
+
+ + + scene_modules + + + + + + + +
+
+
+ Private Area +
+
+
+ + + Private Area + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + +
+
+
+ Public Area +
+
+
+ + + Public Area + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + + + Stopline + + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + + updateSceneModuleInstance + + + + + + + + +
+
+
+ + add/delete modules + +
+
+
+ + + add/delete modules + + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + + modifyPathVelocity + + + + + + + +
+
+
+ + scene_modules + +
+
+
+ + + scene_modules + + + + + + + +
+
+
+ Stopline 1 +
+
+
+ + + Stopline 1 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + +
+
+
+ Stoplin 2 +
+
+
+ + + Stoplin 2 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + + + + + + + + +
+
+
+ + /planning/behavior_planning/path_with_lane_id + +
+
+
+ + + /pla... + + + + + + + + + Behavior Velocity Planner + + + + + + +
+
+
+ UpdateModuleInstance +
+
+
+ + + UpdateModuleInstance + + + + + + + +
+
+
+ PlanVelocity +
+
+
+ + + PlanVelocity + + + + + + + +
+
+
+ RefinePath +
+
+
+ + + RefinePath + + + + + + + + +
+
+
+ path_with_lane_id +
+
+
+ + + path_w... + + + + + + + + +
+
+
+ + path_with_lane_id + +
+
+
+ + + path_w... + + + + + + + + +
+
+
+ path_with_lane_id +
+
+
+ + + path_w... + + + + + + + + +
+
+
+ Updated +
+ + path_with_lane_id + +
+
+
+ + + Update... + + + + + + + + +
+
+
+ + Updated + +
+ path_with_lane_id +
+
+
+ + + Update... + + + + + + + + +
+
+
+ Data +
+
+
+ + + Data + + + + + + + + +
+
+
+ /tf +
+
+
+ + + /tf + + + + + + + + +
+
+
+ /perception/object_recognition/objects +
+
+
+ + + /per... + + + + + + + + +
+
+
+ /perception/prediction/objects +
+
+
+ + + /per... + + + + + + + + +
+
+
+ /vehicle/status/twist +
+
+
+ + + /veh... + + + + + + + + +
+
+
+ /map/semantic +
+
+
+ + + /map... + + + + + + + +
+
+
+ Planner Data +
+
+
+ + + Planner Data + + + + + + + + + SceneManagers + + + + + + + + Crosswalk + + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + + updateSceneModuleInstance + + + + + + + + +
+
+
+ + add/delete modules + +
+
+
+ + + add/delete modules + + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + + modifyPathVelocity + + + + + + + +
+
+
+ + scene_modules + +
+
+
+ + + scene_modules + + + + + + + +
+
+
+ Crosswalk 1 +
+
+
+ + + Crosswalk 1 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + +
+
+
+ Crosswalk2 +
+
+
+ + + Crosswalk2 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + + + + + + + + + Intersection + + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + + updateSceneModuleInstance + + + + + + + + +
+
+
+ + add/delete modules + +
+
+
+ + + add/delete modules + + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + + modifyPathVelocity + + + + + + + +
+
+
+ + scene_modules + +
+
+
+ + + scene_modules + + + + + + + +
+
+
+ Intersection 1 +
+
+
+ + + Intersection 1 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + +
+
+
+ Intersection 2 +
+
+
+ + + Intersection 2 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + Virtual Traffic Light + + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + + updateSceneModuleInstance + + + + + + + + +
+
+
+ + add/delete modules + +
+
+
+ + + add/delete modules + + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + + modifyPathVelocity + + + + + + + +
+
+
+ + scene_modules + +
+
+
+ + + scene_modules + + + + + + + +
+
+
+ Virtual Traffic Light 1 +
+
+
+ + + Virtual Traffic Light 1 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + +
+
+
+ Virtual Traffic Light 2 +
+
+
+ + + Virtual Traffic Light 2 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + Traffic LIght + + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + + updateSceneModuleInstance + + + + + + + + +
+
+
+ + add/delete modules + +
+
+
+ + + add/delete modules + + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + + modifyPathVelocity + + + + + + + +
+
+
+ + scene_modules + +
+
+
+ + + scene_modules + + + + + + + +
+
+
+ Traffic LIght 1 +
+
+
+ + + Traffic LIght 1 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + +
+
+
+ Traffic Ligh 2 +
+
+
+ + + Traffic Ligh 2 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + + + + + No Stopping Area + + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + + updateSceneModuleInstance + + + + + + + + +
+
+
+ + add/delete modules + +
+
+
+ + + add/delete modules + + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + + modifyPathVelocity + + + + + + + +
+
+
+ + scene_modules + +
+
+
+ + + scene_modules + + + + + + + +
+
+
+ No Stopping Area1 +
+
+
+ + + No Stopping Area1 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + + + + +
+
+
+ No Stopping Area2 +
+
+
+ + + No Stopping Area2 + + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + + Update... + + + + + + + + +
+
+
+ + /planning/behavior_planning/path + +
+
+
+ + + /pla... + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/virtual_traffic_light/V2X_support_traffic_light.png b/planning/behavior_velocity_planner/docs/virtual_traffic_light/V2X_support_traffic_light.png new file mode 100644 index 0000000000000..19240ca642242 Binary files /dev/null and b/planning/behavior_velocity_planner/docs/virtual_traffic_light/V2X_support_traffic_light.png differ diff --git a/planning/behavior_velocity_planner/docs/virtual_traffic_light/intersection-coordination.png b/planning/behavior_velocity_planner/docs/virtual_traffic_light/intersection-coordination.png new file mode 100644 index 0000000000000..2e9b751293f44 Binary files /dev/null and b/planning/behavior_velocity_planner/docs/virtual_traffic_light/intersection-coordination.png differ diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index a1f86fdf19419..16998b4f27f19 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -42,7 +42,7 @@ vehicle_info_util visualization_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/virtual-traffic-light-design.md b/planning/behavior_velocity_planner/virtual-traffic-light-design.md new file mode 100644 index 0000000000000..af967c39e45be --- /dev/null +++ b/planning/behavior_velocity_planner/virtual-traffic-light-design.md @@ -0,0 +1,267 @@ +### Virtual Traffic Light + +#### Role + +Autonomous vehicles have to cooperate with the infrastructures such as: + +- Warehouse shutters +- Traffic lights with V2X support +- Communication devices at intersections +- Fleet Management Systems (FMS) + +The following items are example cases: + +1. Traffic control by traffic lights with V2X support + ![traffic_light](docs/virtual_traffic_light/V2X_support_traffic_light.png) + +2. Intersection coordination of multiple vehicles by FMS. + ![FMS](docs/virtual_traffic_light/intersection-coordination.png) + +It's possible to make each function individually, however, the use cases can be generalized with these three elements. + +1. `start`: Start a cooperation procedure after the vehicle enters a certain zone. +2. `stop`: Stop at a defined stop line according to the status received from infrastructures. +3. `end`: Finalize the cooperation procedure after the vehicle reaches the exit zone. This should be done within the range of stable communication. + +This module sends/receives status from infrastructures and plans the velocity of the cooperation result. + +### System Configuration Diagram + +```plantuml +@startuml +!theme cerulean-outline + +' Component +node "Autoware ECU" as autoware_ecu { + component "Behavior Planner" as behavior_planner + component "Autoware API" as autoware_api + database "Vector Map" as vector_map + note bottom of vector_map + Communication metadata is stored. + end note +} + +package "Infrastructures" as infrastructures { + node "FMS" as fms + node "Automatic Shutter" as automatic_shutter + node "Manual Shutter" as manual_shutter + node "Remove Controllable Traffic Light" as remote_controllable_traffic_light + node "Warning Light" as warning_light +} + +' Relationship +'' Behavior Planner <-> Autoware API +behavior_planner -up-> autoware_api : infrastructure\n command +autoware_api -down-> behavior_planner : infrastructure\n state + +'' Vector Map +vector_map -left-> behavior_planner : vector map + +'' Autoware API <-> Infrastructure +autoware_api -up-> fms : lock\n request +fms -down-> autoware_api : right-of-way\n state + +autoware_api -up-> automatic_shutter : approach\n notification +automatic_shutter -down-> autoware_api : shutter\n state + +autoware_api -up-> manual_shutter : open/close\n command +manual_shutter -down-> autoware_api : shutter\n state + +autoware_api -up-> remote_controllable_traffic_light : light change\n command +remote_controllable_traffic_light -down-> autoware_api : light\n state + +autoware_api -up-> warning_light : activation\n command +warning_light -down-> autoware_api : warning light\n state + +' Layout +'' Infrastructure +fms -[hidden]right-> automatic_shutter +automatic_shutter -[hidden]right-> manual_shutter +manual_shutter -[hidden]right-> remote_controllable_traffic_light +remote_controllable_traffic_light -[hidden]right-> warning_light + +@enduml +``` + +Planner and each infrastructure communicate with each other using common abstracted messages. + +- Special handling for each infrastructure is not scalable. The interface is defined as an Autoware API. +- The requirements for each infrastructure are slightly different, but will be handled flexibly. + +FMS: Intersection coordination when multiple vehicles are in operation and the relevant lane is occupied + +- Automatic shutter: Open the shutter when approaching/close it when leaving +- Manual shutter: Have the driver open and close the shutter. +- Remote control signal: Have the driver change the signal status to match the direction of travel. +- Warning light: Activate the warning light + +Support different communication methods for different infrastructures + +- HTTP +- Bluetooth +- ZigBee + +Have different meta-information for each geographic location + +- Associated lane ID +- Hardware ID +- Communication method + +FMS: Fleet Management System + +```plantuml +@startuml +!theme cerulean-outline + +' Component +node "Autoware ECU" as autoware_ecu { +component "Behavior Planner" as behavior_planner +component "Autoware API" as autoware_api +component "Web.Auto Agent" as web_auto_agent +note right of web_auto_agent : (fms_bridge) +database "Vector Map" as vector_map + +package "Infrastructure Bridges" as infrastructure_bridges { + component "Automatic Shutter Bridge" as automatic_shutter_bridge + component "Manual Shutter Bridge" as manual_shutter_bridge + component "Remove Controllable Traffic Light Bridge" as remote_controllable_traffic_light_bridge + component "Warning Light Bridge" as warning_light_bridge +} +} + +cloud "FMS" as fms { + component "FMS Gateway" as fms_gateway + + component "Intersection Arbitrator" as intersection_arbitrator + database "Intersection Lock Table" as intersection_lock_table + + component "Vector Map Builder" as vector_map_builder + database "Vector Map Database" as vector_map_database +} + +package "Infrastructures" as infrastructures { + node "Automatic Shutter" as automatic_shutter + node "Manual Shutter" as manual_shutter + node "Remote Controllable Traffic Light" as remote_controllable_traffic_light + node "Warning Light" as warning_light +} + +' Relationship +'' Behavior Planner <-> Autoware API +behavior_planner -up-> autoware_api : infrastructure\n command +autoware_api -down-> behavior_planner : infrastructure state\n as virtual traffic light + +'' Autoware API <-> Web.Auto +autoware_api -up-> web_auto_agent : infrastructure\n command +web_auto_agent -down-> autoware_api : infrastructure state\n as virtual traffic light + +'' Autoware API <-> Infrastructure Bridge +autoware_api -right-> infrastructure_bridges : infrastructure\n command +infrastructure_bridges -left-> autoware_api : infrastructure state\n as virtual traffic light + +'' Infrastructure Bridge <-> Infrastructure +automatic_shutter_bridge -right-> automatic_shutter : approach notification +automatic_shutter -left-> automatic_shutter_bridge : shutter state + +manual_shutter_bridge -right-> manual_shutter : open/close command +manual_shutter -left-> manual_shutter_bridge : shutter state + +remote_controllable_traffic_light_bridge -right-> remote_controllable_traffic_light : light change command +remote_controllable_traffic_light -left-> remote_controllable_traffic_light_bridge : light state + +warning_light_bridge -right-> warning_light : activation command +warning_light -left-> warning_light_bridge : warning light state + +'' Web.Auto +web_auto_agent -up-> fms_gateway : infrastructure\n command +fms_gateway -down-> web_auto_agent : infrastructure state\n as virtual traffic light + +fms_gateway -up-> intersection_arbitrator : lock request +intersection_arbitrator -down-> fms_gateway : right-of-way state + +intersection_arbitrator -up-> intersection_lock_table : lock request +intersection_lock_table -down-> intersection_arbitrator : lock result + +vector_map_builder -down-> vector_map_database : create vector map +vector_map_database -left-> intersection_arbitrator : vector map + +'' Vector Map +vector_map_database .down.> web_auto_agent : vector map +web_auto_agent -left-> vector_map : vector map +vector_map -down-> behavior_planner : vector map + +' Layout +'' Infrastructure Bridge +automatic_shutter_bridge -[hidden]down-> manual_shutter_bridge +manual_shutter_bridge -[hidden]down-> remote_controllable_traffic_light_bridge +remote_controllable_traffic_light_bridge -[hidden]down-> warning_light_bridge + +'' Infrastructure +automatic_shutter -[hidden]down-> manual_shutter +manual_shutter -[hidden]down-> remote_controllable_traffic_light +remote_controllable_traffic_light -[hidden]down-> warning_light + +@enduml +``` + +#### Module Parameters + +| Parameter | Type | Description | +| ------------------------------- | ------ | --------------------------------------------------------------------- | +| `max_delay_sec` | double | [s] maximum allowed delay for command | +| `near_line_distance` | double | [m] threshold distance to stop line to check ego stop. | +| `dead_line_margin` | double | [m] threshold distance that this module continue to insert stop line. | +| `check_timeout_after_stop_line` | bool | [-] check timeout to stop when linkage is disconnected | + +#### Flowchart + +```plantuml +@startuml +!theme cerulean-outline +start + +if (before start line?) then (yes) + stop +else (no) +endif + +if (after end line?) then (yes) + stop +else (no) +endif + +:send command to infrastructure; + +if (no stop line?) then (yes) + stop +else (no) +endif + +:check infrastructure state; + +if (timeout or not received?) then (yes) + :stop at stop line; + stop +else (no) +endif + +if (no right of way?) then (yes) + :stop at stop line; +else (no) +endif + +if (finalization is requested?) then (yes) + if (not finalized?) then (yes) + :stop at end line; + else (no) + endif +else (no) +endif + +stop +@enduml +``` + +##### Known Limits + +- tbd. diff --git a/planning/costmap_generator/CMakeLists.txt b/planning/costmap_generator/CMakeLists.txt index a2c3f70b2f722..648d01ee4c4e4 100644 --- a/planning/costmap_generator/CMakeLists.txt +++ b/planning/costmap_generator/CMakeLists.txt @@ -46,7 +46,7 @@ rclcpp_components_register_node(costmap_generator_node ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_ros REQUIRED) endif() ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml index 289e2be0197cb..070c160d3aa05 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/costmap_generator/package.xml @@ -29,7 +29,7 @@ tf2_ros tier4_planning_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/freespace_planning_algorithms/CMakeLists.txt b/planning/freespace_planning_algorithms/CMakeLists.txt index 093eaffa23bae..a08e89143ef3c 100644 --- a/planning/freespace_planning_algorithms/CMakeLists.txt +++ b/planning/freespace_planning_algorithms/CMakeLists.txt @@ -18,8 +18,8 @@ target_link_libraries(freespace_planning_algorithms ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(freespace_planning_algorithms-test + find_package(ament_cmake_ros REQUIRED) + ament_add_ros_isolated_gtest(freespace_planning_algorithms-test test/src/test_freespace_planning_algorithms.cpp ) target_link_libraries(freespace_planning_algorithms-test diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index 72fc9d5a0dc45..db0ea35e9bb03 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -21,7 +21,7 @@ tf2_geometry_msgs tier4_autoware_utils - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 2ecf683fd3028..31c6420a67870 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -105,7 +105,7 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( for (size_t i = 0; i < output->size(); ++i) { double curvature = 0.0; const size_t start = i > after_decel_index ? i - after_decel_index : 0; - const size_t end = std::min(output->size(), i + before_decel_index); + const size_t end = std::min(output->size(), i + before_decel_index + 1); for (size_t j = start; j < end; ++j) { curvature = std::max(curvature, std::fabs(curvature_v->at(j))); } diff --git a/planning/planning_error_monitor/CMakeLists.txt b/planning/planning_error_monitor/CMakeLists.txt index 8bd30ea1fd005..66932c63e8990 100644 --- a/planning/planning_error_monitor/CMakeLists.txt +++ b/planning/planning_error_monitor/CMakeLists.txt @@ -23,7 +23,7 @@ rclcpp_components_register_node(invalid_trajectory_publisher_node ) if(BUILD_TESTING) - ament_add_gtest(test_planning_error_monitor + ament_add_ros_isolated_gtest(test_planning_error_monitor test/src/test_main.cpp test/src/test_planning_error_monitor_functions.cpp test/src/test_planning_error_monitor_helper.hpp diff --git a/planning/planning_error_monitor/package.xml b/planning/planning_error_monitor/package.xml index b70e2f319cd01..e667c81790875 100644 --- a/planning/planning_error_monitor/package.xml +++ b/planning/planning_error_monitor/package.xml @@ -21,7 +21,7 @@ tier4_autoware_utils visualization_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/planning_evaluator/CMakeLists.txt b/planning/planning_evaluator/CMakeLists.txt index 6641d08cc52a5..6865f8eef7005 100644 --- a/planning/planning_evaluator/CMakeLists.txt +++ b/planning/planning_evaluator/CMakeLists.txt @@ -26,7 +26,7 @@ rclcpp_components_register_node(${PROJECT_NAME}_node ) if(BUILD_TESTING) - ament_add_gtest(test_${PROJECT_NAME} + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_planning_evaluator_node.cpp ) target_link_libraries(test_${PROJECT_NAME} diff --git a/planning/planning_evaluator/package.xml b/planning/planning_evaluator/package.xml index f7e1d5c3480dd..8681543485c21 100644 --- a/planning/planning_evaluator/package.xml +++ b/planning/planning_evaluator/package.xml @@ -23,7 +23,7 @@ tf2_ros tier4_autoware_utils - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt index b40b7c22c98ca..ab61405c9f335 100644 --- a/planning/surround_obstacle_checker/CMakeLists.txt +++ b/planning/surround_obstacle_checker/CMakeLists.txt @@ -18,7 +18,7 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "SurroundObstacleCheckerNode" + PLUGIN "surround_obstacle_checker::SurroundObstacleCheckerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml index b57d7506b0574..afcc532b7c604 100644 --- a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml +++ b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml @@ -9,3 +9,4 @@ # ego stop state stop_state_ego_speed: 0.1 #[m/s] + stop_state_entry_duration_time: 0.1 #[s] diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp index d076e26afc009..2891ac63c5df4 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp @@ -25,8 +25,18 @@ #include #include +namespace surround_obstacle_checker +{ + +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + enum class PoseType : int8_t { NoStart = 0 }; enum class PointType : int8_t { NoStart = 0 }; + class SurroundObstacleCheckerDebugNode { public: @@ -38,16 +48,16 @@ class SurroundObstacleCheckerDebugNode void publish(); private: - rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr stop_reason_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr stop_reason_pub_; double base_link2front_; - visualization_msgs::msg::MarkerArray makeVisualizationMarker(); - tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray(); + MarkerArray makeVisualizationMarker(); + StopReasonArray makeStopReasonArray(); std::shared_ptr stop_obstacle_point_ptr_; std::shared_ptr stop_pose_ptr_; rclcpp::Clock::SharedPtr clock_; }; - +} // namespace surround_obstacle_checker #endif // SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index 7e4bbb9ffd15a..e437787c234ac 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -16,9 +16,9 @@ #define SURROUND_OBSTACLE_CHECKER__NODE_HPP_ #include "surround_obstacle_checker/debug_marker.hpp" -#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" #include +#include #include #include @@ -27,30 +27,31 @@ #include #include #include +#include +#include #include -#include -#include -#include -#include -#include -#include - -#include #include #include #include #include #include +#include #include -using Point2d = boost::geometry::model::d2::point_xy; -using Polygon2d = - boost::geometry::model::polygon; // counter-clockwise, open +namespace surround_obstacle_checker +{ + +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using vehicle_info_util::VehicleInfo; + +using Obstacle = std::pair; enum class State { PASS, STOP }; @@ -59,70 +60,73 @@ class SurroundObstacleCheckerNode : public rclcpp::Node public: explicit SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options); + struct NodeParam + { + bool use_pointcloud; + bool use_dynamic_object; + bool is_surround_obstacle; + // For preventing chattering, + // surround_check_recover_distance_ must be bigger than surround_check_distance_ + double surround_check_recover_distance; + double surround_check_distance; + double state_clear_time; + double stop_state_ego_speed; + double stop_state_entry_duration_time; + }; + private: - void pathCallback(const Trajectory::ConstSharedPtr input_msg); - void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); - void dynamicObjectCallback( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_msg); - void currentVelocityCallback(const nav_msgs::msg::Odometry::ConstSharedPtr input_msg); - void insertStopVelocity(const size_t closest_idx, TrajectoryPoints * traj); - bool convertPose( - const geometry_msgs::msg::Pose & pose, const std::string & source, const std::string & target, - const rclcpp::Time & time, geometry_msgs::msg::Pose & conv_pose); - bool getPose( - const std::string & source, const std::string & target, geometry_msgs::msg::Pose & pose); - void getNearestObstacle(double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point); - void getNearestObstacleByPointCloud( - double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point); - void getNearestObstacleByDynamicObject( - double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point); - bool isObstacleFound(const double min_dist_to_obj); + void onTimer(); + + void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + + void onDynamicObjects(const PredictedObjects::ConstSharedPtr msg); + + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + boost::optional getNearestObstacle() const; + + boost::optional getNearestObstacleByPointCloud() const; + + boost::optional getNearestObstacleByDynamicObject() const; + + boost::optional getTransform( + const std::string & source, const std::string & target, const rclcpp::Time & stamp, + double duration_sec) const; + bool isStopRequired(const bool is_obstacle_found, const bool is_stopped); - size_t getClosestIdx(const TrajectoryPoints & traj, const geometry_msgs::msg::Pose current_pose); - bool checkStop(const TrajectoryPoint & closest_point); - Polygon2d createSelfPolygon(); - Polygon2d createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size); - Polygon2d createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint); - diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose); - std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose); - - /* - * ROS - */ + + bool isVehicleStopped(); + + // ros + mutable tf2_ros::Buffer tf_buffer_{get_clock()}; + mutable tf2_ros::TransformListener tf_listener_{tf_buffer_}; + rclcpp::TimerBase::SharedPtr timer_; + // publisher and subscriber - rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr pointcloud_sub_; - rclcpp::Subscription::SharedPtr - dynamic_object_sub_; - rclcpp::Subscription::SharedPtr current_velocity_sub_; - rclcpp::Publisher::SharedPtr path_pub_; - rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; + rclcpp::Subscription::SharedPtr sub_pointcloud_; + rclcpp::Subscription::SharedPtr sub_dynamic_objects_; + rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Publisher::SharedPtr pub_stop_reason_; + rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + + // debug std::shared_ptr debug_ptr_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; // parameter - nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_; - sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr_; + NodeParam node_param_; vehicle_info_util::VehicleInfo vehicle_info_; - Polygon2d self_poly_; - bool use_pointcloud_; - bool use_dynamic_object_; - double surround_check_distance_; - // For preventing chattering, - // surround_check_recover_distance_ must be bigger than surround_check_distance_ - double surround_check_recover_distance_; - double state_clear_time_; - double stop_state_ego_speed_; - bool is_surround_obstacle_; + + // data + nav_msgs::msg::Odometry::ConstSharedPtr odometry_ptr_; + sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_; + PredictedObjects::ConstSharedPtr object_ptr_; // State Machine State state_ = State::PASS; std::shared_ptr last_obstacle_found_time_; + std::shared_ptr last_running_time_; }; +} // namespace surround_obstacle_checker #endif // SURROUND_OBSTACLE_CHECKER__NODE_HPP_ diff --git a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml b/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml index a0dd4b307f495..53f26a05c87c3 100644 --- a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml +++ b/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml @@ -1,24 +1,20 @@ - - - - - + + - - + + - diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 5dfed9ddb19a7..b74ad2ccec25b 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_auto_tf2 diagnostic_msgs eigen libpcl-all-dev diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index 64dbc2f349f14..8abdb67e8b2c6 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -14,6 +14,7 @@ #include "surround_obstacle_checker/debug_marker.hpp" +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -22,13 +23,23 @@ #include +namespace surround_obstacle_checker +{ + +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::createStopVirtualWallMarker; + SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const double base_link2front, const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) : base_link2front_(base_link2front), clock_(clock) { debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); - stop_reason_pub_ = - node.create_publisher("~/output/stop_reasons", 1); + stop_reason_pub_ = node.create_publisher("~/output/stop_reasons", 1); } bool SurroundObstacleCheckerDebugNode::pushPose( @@ -70,87 +81,25 @@ void SurroundObstacleCheckerDebugNode::publish() stop_obstacle_point_ptr_ = nullptr; } -visualization_msgs::msg::MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() +MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() { - visualization_msgs::msg::MarkerArray msg; + MarkerArray msg; rclcpp::Time current_time = this->clock_->now(); - tf2::Transform tf_base_link2front( - tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(base_link2front_, 0.0, 0.0)); // visualize stop line if (stop_pose_ptr_ != nullptr) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = "virtual_wall/no_start"; - marker.id = 0; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.action = visualization_msgs::msg::Marker::ADD; - tf2::Transform tf_map2base_link; - tf2::fromMsg(*stop_pose_ptr_, tf_map2base_link); - tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; - tf2::toMsg(tf_map2front, marker.pose); - marker.pose.position.z += 1.0; - marker.scale.x = 0.1; - marker.scale.y = 5.0; - marker.scale.z = 2.0; - marker.color.a = 0.5; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - msg.markers.push_back(marker); - } - - // visualize stop reason - if (stop_pose_ptr_ != nullptr) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = "factor_text/no_start"; - marker.id = 0; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.action = visualization_msgs::msg::Marker::ADD; - tf2::Transform tf_map2base_link; - tf2::fromMsg(*stop_pose_ptr_, tf_map2base_link); - tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; - tf2::toMsg(tf_map2front, marker.pose); - marker.pose.position.z += 2.0; - marker.scale.x = 0.0; - marker.scale.y = 0.0; - marker.scale.z = 1.0; - marker.color.a = 0.999; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - marker.text = "surround obstacle"; - msg.markers.push_back(marker); + const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto markers = createStopVirtualWallMarker(p, "surround obstacle", current_time, 0); + appendMarkerArray(markers, &msg); } // visualize surround object if (stop_obstacle_point_ptr_ != nullptr) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = "no_start_obstacle_text"; - marker.id = 0; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.action = visualization_msgs::msg::Marker::ADD; + auto marker = createDefaultMarker( + "map", current_time, "no_start_obstacle_text", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); marker.pose.position = *stop_obstacle_point_ptr_; marker.pose.position.z += 2.0; // add half of the heights of obj roughly - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.0; - marker.scale.y = 0.0; - marker.scale.z = 1.0; - marker.color.a = 0.999; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; marker.text = "!"; msg.markers.push_back(marker); } @@ -158,7 +107,7 @@ visualization_msgs::msg::MarkerArray SurroundObstacleCheckerDebugNode::makeVisua return msg; } -tier4_planning_msgs::msg::StopReasonArray SurroundObstacleCheckerDebugNode::makeStopReasonArray() +StopReasonArray SurroundObstacleCheckerDebugNode::makeStopReasonArray() { // create header std_msgs::msg::Header header; @@ -166,9 +115,9 @@ tier4_planning_msgs::msg::StopReasonArray SurroundObstacleCheckerDebugNode::make header.stamp = this->clock_->now(); // create stop reason stamped - tier4_planning_msgs::msg::StopReason stop_reason_msg; - stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::SURROUND_OBSTACLE_CHECK; - tier4_planning_msgs::msg::StopFactor stop_factor; + StopReason stop_reason_msg; + stop_reason_msg.reason = StopReason::SURROUND_OBSTACLE_CHECK; + StopFactor stop_factor; if (stop_pose_ptr_ != nullptr) { stop_factor.stop_pose = *stop_pose_ptr_; @@ -179,8 +128,10 @@ tier4_planning_msgs::msg::StopReasonArray SurroundObstacleCheckerDebugNode::make } // create stop reason array - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + StopReasonArray stop_reason_array; stop_reason_array.header = header; stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); return stop_reason_array; } + +} // namespace surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 09fe48f35b571..cd7117f360143 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -14,9 +14,17 @@ #include "surround_obstacle_checker/node.hpp" +#include + +#include +#include +#include +#include +#include +#include + #include #include -#include #include #ifdef ROS_DISTRO_GALACTIC #include @@ -34,313 +42,381 @@ #include #include +namespace surround_obstacle_checker +{ + +namespace bg = boost::geometry; +using Point2d = bg::model::d2::point_xy; +using Polygon2d = bg::model::polygon; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::pose2transform; + +namespace +{ +std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) +{ + const std::string json_dumps_pose = + (boost::format( + R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % + pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % + pose.orientation.y % pose.orientation.z) + .str(); + return json_dumps_pose; +} + +diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( + const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose) +{ + diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; + diagnostic_msgs::msg::KeyValue no_start_reason_diag_kv; + no_start_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + no_start_reason_diag.name = "no_start_reason"; + no_start_reason_diag.message = no_start_reason; + no_start_reason_diag_kv.key = "no_start_pose"; + no_start_reason_diag_kv.value = jsonDumpsPose(stop_pose); + no_start_reason_diag.values.push_back(no_start_reason_diag_kv); + return no_start_reason_diag; +} + +geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const double z) +{ + geometry_msgs::msg::Point32 p; + p.x = x; + p.y = y; + p.z = z; + return p; +} + +Polygon2d createObjPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint) +{ + geometry_msgs::msg::Polygon transformed_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(pose); + tf2::doTransform(footprint, transformed_polygon, geometry_tf); + + Polygon2d object_polygon; + for (const auto & p : transformed_polygon.points) { + object_polygon.outer().push_back(Point2d(p.x, p.y)); + } + + bg::correct(object_polygon); + + return object_polygon; +} + +Polygon2d createObjPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) +{ + const double & length_m = size.x / 2.0; + const double & width_m = size.y / 2.0; + + geometry_msgs::msg::Polygon polygon{}; + + polygon.points.push_back(createPoint32(length_m, -width_m, 0.0)); + polygon.points.push_back(createPoint32(length_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-length_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-length_m, -width_m, 0.0)); + + return createObjPolygon(pose, polygon); +} + +Polygon2d createSelfPolygon(const VehicleInfo & vehicle_info) +{ + const double & front_m = vehicle_info.max_longitudinal_offset_m; + const double & width_m = vehicle_info.min_lateral_offset_m; + const double & rear_m = vehicle_info.min_longitudinal_offset_m; + + Polygon2d ego_polygon; + + ego_polygon.outer().push_back(Point2d(front_m, -width_m)); + ego_polygon.outer().push_back(Point2d(front_m, width_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_m)); + ego_polygon.outer().push_back(Point2d(rear_m, -width_m)); + + bg::correct(ego_polygon); + + return ego_polygon; +} +} // namespace + SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options) -: Node("surround_obstacle_checker_node", node_options), - tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) +: Node("surround_obstacle_checker_node", node_options) { // Parameters - use_pointcloud_ = this->declare_parameter("use_pointcloud", true); - use_dynamic_object_ = this->declare_parameter("use_dynamic_object", true); - surround_check_distance_ = this->declare_parameter("surround_check_distance", 2.0); - surround_check_recover_distance_ = - this->declare_parameter("surround_check_recover_distance", 2.5); - state_clear_time_ = this->declare_parameter("state_clear_time", 2.0); - stop_state_ego_speed_ = this->declare_parameter("stop_state_ego_speed", 0.1); - debug_ptr_ = std::make_shared( - vehicle_info_.max_longitudinal_offset_m, this->get_clock(), *this); - self_poly_ = createSelfPolygon(); + { + auto & p = node_param_; + p.use_pointcloud = this->declare_parameter("use_pointcloud", true); + p.use_dynamic_object = this->declare_parameter("use_dynamic_object", true); + p.surround_check_distance = this->declare_parameter("surround_check_distance", 2.0); + p.surround_check_recover_distance = + this->declare_parameter("surround_check_recover_distance", 2.5); + p.state_clear_time = this->declare_parameter("state_clear_time", 2.0); + p.stop_state_ego_speed = this->declare_parameter("stop_state_ego_speed", 0.1); + p.stop_state_entry_duration_time = + this->declare_parameter("stop_state_entry_duration_time", 0.1); + } + + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); // Publishers - path_pub_ = - this->create_publisher("~/output/trajectory", 1); - stop_reason_diag_pub_ = + pub_stop_reason_ = this->create_publisher("~/output/no_start_reason", 1); + pub_clear_velocity_limit_ = + this->create_publisher("~/output/velocity_limit_clear_command", 1); + pub_velocity_limit_ = this->create_publisher("~/output/max_velocity", 1); - // Subscriber - path_sub_ = this->create_subscription( - "~/input/trajectory", 1, - std::bind(&SurroundObstacleCheckerNode::pathCallback, this, std::placeholders::_1)); - pointcloud_sub_ = this->create_subscription( + // Subscribers + sub_pointcloud_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&SurroundObstacleCheckerNode::pointCloudCallback, this, std::placeholders::_1)); - dynamic_object_sub_ = - this->create_subscription( - "~/input/objects", 1, - std::bind(&SurroundObstacleCheckerNode::dynamicObjectCallback, this, std::placeholders::_1)); - current_velocity_sub_ = this->create_subscription( + std::bind(&SurroundObstacleCheckerNode::onPointCloud, this, std::placeholders::_1)); + sub_dynamic_objects_ = this->create_subscription( + "~/input/objects", 1, + std::bind(&SurroundObstacleCheckerNode::onDynamicObjects, this, std::placeholders::_1)); + sub_odometry_ = this->create_subscription( "~/input/odometry", 1, - std::bind(&SurroundObstacleCheckerNode::currentVelocityCallback, this, std::placeholders::_1)); + std::bind(&SurroundObstacleCheckerNode::onOdometry, this, std::placeholders::_1)); + + using std::chrono_literals::operator""ms; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&SurroundObstacleCheckerNode::onTimer, this)); + + last_running_time_ = std::make_shared(this->now()); + + // Debug + debug_ptr_ = std::make_shared( + vehicle_info_.max_longitudinal_offset_m, this->get_clock(), *this); } -void SurroundObstacleCheckerNode::pathCallback( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) +void SurroundObstacleCheckerNode::onTimer() { - if (use_pointcloud_ && !pointcloud_ptr_) { + if (node_param_.use_pointcloud && !pointcloud_ptr_) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for pointcloud info..."); return; } - if (use_dynamic_object_ && !object_ptr_) { + if (node_param_.use_dynamic_object && !object_ptr_) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for dynamic object info..."); return; } - if (!current_velocity_ptr_) { + if (!odometry_ptr_) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for current velocity..."); return; } - // parameter description - TrajectoryPoints output_trajectory_points = - tier4_autoware_utils::convertToTrajectoryPointArray(*input_msg); + const auto nearest_obstacle = getNearestObstacle(); + const auto is_vehicle_stopped = isVehicleStopped(); - diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; + switch (state_) { + case State::PASS: { + const auto is_obstacle_found = + !nearest_obstacle ? false + : nearest_obstacle.get().first < node_param_.surround_check_distance; - // get current pose in traj frame - geometry_msgs::msg::Pose current_pose; - if (!getPose(input_msg->header.frame_id, "base_link", current_pose)) { - return; - } + if (!isStopRequired(is_obstacle_found, is_vehicle_stopped)) { + break; + } + + state_ = State::STOP; - // get closest idx - const size_t closest_idx = getClosestIdx(output_trajectory_points, current_pose); + auto velocity_limit = std::make_shared(); + velocity_limit->stamp = this->now(); + velocity_limit->max_velocity = 0.0; + velocity_limit->use_constraints = false; + velocity_limit->sender = "surround_obstacle_checker"; - // get nearest object - double min_dist_to_obj = std::numeric_limits::max(); - geometry_msgs::msg::Point nearest_obj_point; - getNearestObstacle(&min_dist_to_obj, &nearest_obj_point); + pub_velocity_limit_->publish(*velocity_limit); - // check current obstacle status (exist or not) - const auto is_obstacle_found = isObstacleFound(min_dist_to_obj); + // do not start when there is a obstacle near the ego vehicle. + RCLCPP_WARN(get_logger(), "do not start because there is obstacle near the ego vehicle."); - // check current stop status (stop or not) - const auto is_stopped = checkStop(input_msg->points.at(closest_idx)); + break; + } - const auto is_stop_required = isStopRequired(is_obstacle_found, is_stopped); + case State::STOP: { + const auto is_obstacle_found = + !nearest_obstacle + ? false + : nearest_obstacle.get().first < node_param_.surround_check_recover_distance; - // insert stop velocity - if (is_stop_required) { - state_ = State::STOP; + if (isStopRequired(is_obstacle_found, is_vehicle_stopped)) { + break; + } - // do not start when there is a obstacle near the ego vehicle. - RCLCPP_WARN_THROTTLE( - get_logger(), *this->get_clock(), 500 /* ms */, - "do not start because there is obstacle near the ego vehicle."); - insertStopVelocity(closest_idx, &output_trajectory_points); + state_ = State::PASS; + + auto velocity_limit_clear_command = std::make_shared(); + velocity_limit_clear_command->stamp = this->now(); + velocity_limit_clear_command->command = true; + velocity_limit_clear_command->sender = "surround_obstacle_checker"; - // visualization for debug - if (is_obstacle_found) { - debug_ptr_->pushObstaclePoint(nearest_obj_point, PointType::NoStart); + pub_clear_velocity_limit_->publish(*velocity_limit_clear_command); + + break; } - debug_ptr_->pushPose(input_msg->points.at(closest_idx).pose, PoseType::NoStart); - no_start_reason_diag = makeStopReasonDiag("obstacle", input_msg->points.at(closest_idx).pose); - } else { - state_ = State::PASS; + + default: + break; } - // publish trajectory and debug info - auto output_msg = tier4_autoware_utils::convertToTrajectory(output_trajectory_points); - output_msg.header = input_msg->header; - path_pub_->publish(output_msg); - stop_reason_diag_pub_->publish(no_start_reason_diag); + if (nearest_obstacle) { + debug_ptr_->pushObstaclePoint(nearest_obstacle.get().second, PointType::NoStart); + } + + diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; + if (state_ == State::STOP) { + debug_ptr_->pushPose(odometry_ptr_->pose.pose, PoseType::NoStart); + no_start_reason_diag = makeStopReasonDiag("obstacle", odometry_ptr_->pose.pose); + } + + pub_stop_reason_->publish(no_start_reason_diag); debug_ptr_->publish(); } -void SurroundObstacleCheckerNode::pointCloudCallback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) +void SurroundObstacleCheckerNode::onPointCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { - pointcloud_ptr_ = input_msg; + pointcloud_ptr_ = msg; } -void SurroundObstacleCheckerNode::dynamicObjectCallback( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_msg) +void SurroundObstacleCheckerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr msg) { - object_ptr_ = input_msg; + object_ptr_ = msg; } -void SurroundObstacleCheckerNode::currentVelocityCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr input_msg) +void SurroundObstacleCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { - current_velocity_ptr_ = input_msg; + odometry_ptr_ = msg; } -void SurroundObstacleCheckerNode::insertStopVelocity( - const size_t closest_idx, TrajectoryPoints * traj) +boost::optional SurroundObstacleCheckerNode::getNearestObstacle() const { - // set zero velocity from closest idx to last idx - for (size_t i = closest_idx; i < traj->size(); i++) { - traj->at(i).longitudinal_velocity_mps = 0.0; - } -} + boost::optional nearest_pointcloud{boost::none}; + boost::optional nearest_object{boost::none}; -bool SurroundObstacleCheckerNode::getPose( - const std::string & source, const std::string & target, geometry_msgs::msg::Pose & pose) -{ - try { - // get transform from source to target - geometry_msgs::msg::TransformStamped ros_src2tgt = - tf_buffer_.lookupTransform(source, target, tf2::TimePointZero); - // convert geometry_msgs::msg::Transform to geometry_msgs::msg::Pose - tf2::Transform transform; - tf2::fromMsg(ros_src2tgt.transform, transform); - tf2::toMsg(transform, pose); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *this->get_clock(), 500 /* ms */, - "cannot get tf from " << source << " to " << target); - return false; + if (node_param_.use_pointcloud) { + nearest_pointcloud = getNearestObstacleByPointCloud(); } - return true; -} -bool SurroundObstacleCheckerNode::convertPose( - const geometry_msgs::msg::Pose & pose, const std::string & source, const std::string & target, - const rclcpp::Time & time, geometry_msgs::msg::Pose & conv_pose) -{ - tf2::Transform src2tgt; - try { - // get transform from source to target - geometry_msgs::msg::TransformStamped ros_src2tgt = - tf_buffer_.lookupTransform(source, target, time, tf2::durationFromSec(0.1)); - tf2::fromMsg(ros_src2tgt.transform, src2tgt); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *this->get_clock(), 500 /* ms */, - "cannot get tf from " << source << " to " << target); - return false; + if (node_param_.use_dynamic_object) { + nearest_object = getNearestObstacleByDynamicObject(); } - tf2::Transform src2obj; - tf2::fromMsg(pose, src2obj); - tf2::Transform tgt2obj = src2tgt.inverse() * src2obj; - tf2::toMsg(tgt2obj, conv_pose); - return true; -} - -size_t SurroundObstacleCheckerNode::getClosestIdx( - const TrajectoryPoints & traj, const geometry_msgs::msg::Pose current_pose) -{ - double min_dist = std::numeric_limits::max(); - size_t min_dist_idx = 0; - for (size_t i = 0; i < traj.size(); ++i) { - const double x = traj.at(i).pose.position.x - current_pose.position.x; - const double y = traj.at(i).pose.position.y - current_pose.position.y; - const double dist = std::hypot(x, y); - if (dist < min_dist) { - min_dist_idx = i; - min_dist = dist; - } + if (!nearest_pointcloud && !nearest_object) { + return {}; } - return min_dist_idx; -} -void SurroundObstacleCheckerNode::getNearestObstacle( - double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point) -{ - if (use_pointcloud_) { - getNearestObstacleByPointCloud(min_dist_to_obj, nearest_obj_point); + if (!nearest_pointcloud) { + return nearest_object; } - if (use_dynamic_object_) { - getNearestObstacleByDynamicObject(min_dist_to_obj, nearest_obj_point); + if (!nearest_object) { + return nearest_pointcloud; } + + return nearest_pointcloud.get().first < nearest_object.get().first ? nearest_pointcloud + : nearest_object; } -void SurroundObstacleCheckerNode::getNearestObstacleByPointCloud( - double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point) +boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const { - // wait to transform pointcloud - geometry_msgs::msg::TransformStamped transform_stamped; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, - tf2::durationFromSec(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *this->get_clock(), 500 /* ms */, - "failed to get base_link to " << pointcloud_ptr_->header.frame_id << " transform."); - return; + const auto transform_stamped = + getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5); + + geometry_msgs::msg::Point nearest_point; + auto minimum_distance = std::numeric_limits::max(); + + if (!transform_stamped) { + return {}; } - Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.transform).cast(); - pcl::PointCloud pcl; - pcl::fromROSMsg(*pointcloud_ptr_, pcl); - pcl::transformPointCloud(pcl, pcl, isometry); - for (const auto & p : pcl) { - // create boost point - Point2d boost_p(p.x, p.y); - - // calc distance - const double dist_to_obj = boost::geometry::distance(self_poly_, boost_p); - - // get minimum distance to obj - if (dist_to_obj < *min_dist_to_obj) { - *min_dist_to_obj = dist_to_obj; - nearest_obj_point->x = p.x; - nearest_obj_point->y = p.y; - nearest_obj_point->z = p.z; + Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.get().transform).cast(); + pcl::PointCloud transformed_pointcloud; + pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud); + pcl::transformPointCloud(transformed_pointcloud, transformed_pointcloud, isometry); + + const auto ego_polygon = createSelfPolygon(vehicle_info_); + + for (const auto & p : transformed_pointcloud) { + Point2d boost_point(p.x, p.y); + + const auto distance_to_object = bg::distance(ego_polygon, boost_point); + + if (distance_to_object < minimum_distance) { + nearest_point = createPoint(p.x, p.y, p.z); + minimum_distance = distance_to_object; } } + + return std::make_pair(minimum_distance, nearest_point); } -void SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject( - double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point) +boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject() const { - const auto obj_frame = object_ptr_->header.frame_id; - const auto obj_time = object_ptr_->header.stamp; - for (const auto & obj : object_ptr_->objects) { - // change frame of obj_pose to base_link - geometry_msgs::msg::Pose pose_baselink; - if (!convertPose( - obj.kinematics.initial_pose_with_covariance.pose, obj_frame, "base_link", obj_time, - pose_baselink)) { - return; - } + const auto transform_stamped = + getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); - // create obj polygon - Polygon2d obj_poly; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - obj_poly = createObjPolygon(pose_baselink, obj.shape.footprint); - } else { - obj_poly = createObjPolygon(pose_baselink, obj.shape.dimensions); - } + geometry_msgs::msg::Point nearest_point; + auto minimum_distance = std::numeric_limits::max(); + + if (!transform_stamped) { + return {}; + } + + tf2::Transform tf_src2target; + tf2::fromMsg(transform_stamped.get().transform, tf_src2target); + + const auto ego_polygon = createSelfPolygon(vehicle_info_); - // calc distance - const double dist_to_obj = boost::geometry::distance(self_poly_, obj_poly); + for (const auto & object : object_ptr_->objects) { + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - // get minimum distance to obj - if (dist_to_obj < *min_dist_to_obj) { - *min_dist_to_obj = dist_to_obj; - *nearest_obj_point = obj.kinematics.initial_pose_with_covariance.pose.position; + tf2::Transform tf_src2object; + tf2::fromMsg(object_pose, tf_src2object); + + geometry_msgs::msg::Pose transformed_object_pose; + tf2::toMsg(tf_src2target.inverse() * tf_src2object, transformed_object_pose); + + const auto object_polygon = + object.shape.type == Shape::POLYGON + ? createObjPolygon(transformed_object_pose, object.shape.footprint) + : createObjPolygon(transformed_object_pose, object.shape.dimensions); + + const auto distance_to_object = bg::distance(ego_polygon, object_polygon); + + if (distance_to_object < minimum_distance) { + nearest_point = object_pose.position; + minimum_distance = distance_to_object; } } + + return std::make_pair(minimum_distance, nearest_point); } -bool SurroundObstacleCheckerNode::isObstacleFound(const double min_dist_to_obj) +boost::optional SurroundObstacleCheckerNode::getTransform( + const std::string & source, const std::string & target, const rclcpp::Time & stamp, + double duration_sec) const { - const auto is_obstacle_inside_range = min_dist_to_obj < surround_check_distance_; - const auto is_obstacle_outside_range = min_dist_to_obj > surround_check_recover_distance_; - - if (state_ == State::PASS) { - return is_obstacle_inside_range; - } + geometry_msgs::msg::TransformStamped transform_stamped; - if (state_ == State::STOP) { - return !is_obstacle_outside_range; + try { + transform_stamped = + tf_buffer_.lookupTransform(source, target, stamp, tf2::durationFromSec(duration_sec)); + } catch (tf2::TransformException & ex) { + return {}; } - throw std::runtime_error("invalid state"); + return transform_stamped; } bool SurroundObstacleCheckerNode::isStopRequired( - const bool is_obstacle_found, const bool is_stopped) + const bool is_obstacle_found, const bool is_vehicle_stopped) { - if (!is_stopped) { + if (!is_vehicle_stopped) { return false; } @@ -356,7 +432,7 @@ bool SurroundObstacleCheckerNode::isStopRequired( // Keep stop state if (last_obstacle_found_time_) { const auto elapsed_time = this->now() - *last_obstacle_found_time_; - if (elapsed_time.seconds() <= state_clear_time_) { + if (elapsed_time.seconds() <= node_param_.state_clear_time) { return true; } } @@ -365,118 +441,18 @@ bool SurroundObstacleCheckerNode::isStopRequired( return false; } -bool SurroundObstacleCheckerNode::checkStop( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & closest_point) -{ - if (std::fabs(current_velocity_ptr_->twist.twist.linear.x) > stop_state_ego_speed_) { - // ego vehicle has high velocity now. not stop. - return false; - } - - const double closest_vel = closest_point.longitudinal_velocity_mps; - const double closest_acc = closest_point.acceleration_mps2; - - if (closest_vel * closest_acc < 0) { - // ego vehicle is about to stop (during deceleration). not stop. - return false; - } - - return true; -} - -Polygon2d SurroundObstacleCheckerNode::createSelfPolygon() -{ - const double front = vehicle_info_.max_longitudinal_offset_m; - const double rear = vehicle_info_.min_longitudinal_offset_m; - const double left = vehicle_info_.max_lateral_offset_m; - const double right = vehicle_info_.min_lateral_offset_m; - - Polygon2d poly; - boost::geometry::exterior_ring(poly) = boost::assign::list_of(front, left)(front, right)( - rear, right)(rear, left)(front, left); - return poly; -} - -Polygon2d SurroundObstacleCheckerNode::createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) +bool SurroundObstacleCheckerNode::isVehicleStopped() { - // rename - const double x = pose.position.x; - const double y = pose.position.y; - const double h = size.x; - const double w = size.y; - const double yaw = tf2::getYaw(pose.orientation); - - // create base polygon - Polygon2d obj_poly; - boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of(h / 2.0, w / 2.0)( - -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0); - - // rotate polygon(yaw) - boost::geometry::strategy::transform::rotate_transformer - rotate(-yaw); // anti-clockwise -> :clockwise rotation - Polygon2d rotate_obj_poly; - boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); - - // translate polygon(x, y) - boost::geometry::strategy::transform::translate_transformer translate(x, y); - Polygon2d translate_obj_poly; - boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); - return translate_obj_poly; -} + const auto current_velocity = std::abs(odometry_ptr_->twist.twist.linear.x); -Polygon2d SurroundObstacleCheckerNode::createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint) -{ - // rename - const double x = pose.position.x; - const double y = pose.position.y; - const double yaw = tf2::getYaw(pose.orientation); - - // create base polygon - Polygon2d obj_poly; - for (const auto point : footprint.points) { - const Point2d point2d(point.x, point.y); - obj_poly.outer().push_back(point2d); + if (node_param_.stop_state_ego_speed < current_velocity) { + last_running_time_ = std::make_shared(this->now()); } - // rotate polygon(yaw) - boost::geometry::strategy::transform::rotate_transformer - rotate(-yaw); // anti-clockwise -> :clockwise rotation - Polygon2d rotate_obj_poly; - boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); - - // translate polygon(x, y) - boost::geometry::strategy::transform::translate_transformer translate(x, y); - Polygon2d translate_obj_poly; - boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); - return translate_obj_poly; -} - -diagnostic_msgs::msg::DiagnosticStatus SurroundObstacleCheckerNode::makeStopReasonDiag( - const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose) -{ - diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; - diagnostic_msgs::msg::KeyValue no_start_reason_diag_kv; - no_start_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - no_start_reason_diag.name = "no_start_reason"; - no_start_reason_diag.message = no_start_reason; - no_start_reason_diag_kv.key = "no_start_pose"; - no_start_reason_diag_kv.value = jsonDumpsPose(stop_pose); - no_start_reason_diag.values.push_back(no_start_reason_diag_kv); - return no_start_reason_diag; + return node_param_.stop_state_entry_duration_time < (this->now() - *last_running_time_).seconds(); } -std::string SurroundObstacleCheckerNode::jsonDumpsPose(const geometry_msgs::msg::Pose & pose) -{ - const std::string json_dumps_pose = - (boost::format( - R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % - pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % - pose.orientation.y % pose.orientation.z) - .str(); - return json_dumps_pose; -} +} // namespace surround_obstacle_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(SurroundObstacleCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(surround_obstacle_checker::SurroundObstacleCheckerNode) diff --git a/simulator/dummy_perception_publisher/CMakeLists.txt b/simulator/dummy_perception_publisher/CMakeLists.txt index 85347b04b3be6..63d07faf0634c 100644 --- a/simulator/dummy_perception_publisher/CMakeLists.txt +++ b/simulator/dummy_perception_publisher/CMakeLists.txt @@ -74,7 +74,7 @@ ament_auto_add_executable(empty_objects_publisher ) if(BUILD_TESTING) - ament_add_gtest(signed_distance_function-test + ament_add_ros_isolated_gtest(signed_distance_function-test test/src/test_signed_distance_function.cpp ) target_link_libraries(signed_distance_function-test diff --git a/simulator/dummy_perception_publisher/package.xml b/simulator/dummy_perception_publisher/package.xml index e7ce4dc99c433..1635deda40a4b 100644 --- a/simulator/dummy_perception_publisher/package.xml +++ b/simulator/dummy_perception_publisher/package.xml @@ -14,6 +14,7 @@ rosidl_default_runtime + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp index 6c964625a3e69..d55c42a0c28bd 100644 --- a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp +++ b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp @@ -25,6 +25,18 @@ #include #include +namespace +{ + +static constexpr double epsilon = 0.001; +static constexpr double step = 0.05; +static constexpr double vertical_theta_step = (1.0 / 180.0) * M_PI; +static constexpr double vertical_min_theta = (-15.0 / 180.0) * M_PI; +static constexpr double vertical_max_theta = (15.0 / 180.0) * M_PI; +static constexpr double horizontal_theta_step = (0.1 / 180.0) * M_PI; +static constexpr double horizontal_min_theta = (-180.0 / 180.0) * M_PI; +static constexpr double horizontal_max_theta = (180.0 / 180.0) * M_PI; + pcl::PointXYZ getPointWrtBaseLink( const tf2::Transform & tf_base_link2moved_object, double x, double y, double z) { @@ -32,6 +44,8 @@ pcl::PointXYZ getPointWrtBaseLink( return pcl::PointXYZ(p_wrt_base.x(), p_wrt_base.y(), p_wrt_base.z()); } +} // namespace + void ObjectCentricPointCloudCreator::create_object_pointcloud( const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) const @@ -39,14 +53,6 @@ void ObjectCentricPointCloudCreator::create_object_pointcloud( std::normal_distribution<> x_random(0.0, obj_info.std_dev_x); std::normal_distribution<> y_random(0.0, obj_info.std_dev_y); std::normal_distribution<> z_random(0.0, obj_info.std_dev_z); - const double epsilon = 0.001; - const double step = 0.05; - const double vertical_theta_step = (1.0 / 180.0) * M_PI; - const double vertical_min_theta = (-15.0 / 180.0) * M_PI; - const double vertical_max_theta = (15.0 / 180.0) * M_PI; - const double horizontal_theta_step = (0.1 / 180.0) * M_PI; - const double horizontal_min_theta = (-180.0 / 180.0) * M_PI; - const double horizontal_max_theta = (180.0 / 180.0) * M_PI; const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object; @@ -206,7 +212,18 @@ std::vector::Ptr> EgoCentricPointCloudCreator::cr pointclouds.at(i) = (pcl::PointCloud::Ptr(new pcl::PointCloud)); } - const double horizontal_theta_step = 0.25 * M_PI / 180.0; + std::vector min_zs(obj_infos.size()); + std::vector max_zs(obj_infos.size()); + + for (size_t idx = 0; idx < obj_infos.size(); ++idx) { + const auto & obj_info = obj_infos.at(idx); + const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object; + const double min_z = -1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z(); + const double max_z = 1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z(); + min_zs.at(idx) = min_z; + max_zs.at(idx) = max_z; + } + double angle = 0.0; const auto n_scan = static_cast(std::floor(2 * M_PI / horizontal_theta_step)); for (size_t i = 0; i < n_scan; ++i) { @@ -217,13 +234,22 @@ std::vector::Ptr> EgoCentricPointCloudCreator::cr const auto x_hit = dist * cos(angle); const auto y_hit = dist * sin(angle); const auto idx_hit = composite_sdf.nearest_sdf_index(x_hit, y_hit); + const auto obj_info_here = obj_infos.at(idx_hit); + const auto min_z_here = min_zs.at(idx_hit); + const auto max_z_here = max_zs.at(idx_hit); + std::normal_distribution<> x_random(0.0, obj_info_here.std_dev_x); + std::normal_distribution<> y_random(0.0, obj_info_here.std_dev_y); + std::normal_distribution<> z_random(0.0, obj_info_here.std_dev_z); - std::normal_distribution<> x_random(0.0, obj_infos.at(idx_hit).std_dev_x); - std::normal_distribution<> y_random(0.0, obj_infos.at(idx_hit).std_dev_y); - std::normal_distribution<> z_random(0.0, obj_infos.at(idx_hit).std_dev_z); - pointclouds.at(idx_hit)->push_back(pcl::PointXYZ( - x_hit + x_random(random_generator), y_hit + y_random(random_generator), - z_random(random_generator))); + for (double vertical_theta = vertical_min_theta; + vertical_theta <= vertical_max_theta + epsilon; vertical_theta += vertical_theta_step) { + const double z = dist * std::tan(vertical_theta); + if (min_z_here <= z && z <= max_z_here + epsilon) { + pointclouds.at(idx_hit)->push_back(pcl::PointXYZ( + x_hit + x_random(random_generator), y_hit + y_random(random_generator), + z + z_random(random_generator))); + } + } } } diff --git a/simulator/fault_injection/CMakeLists.txt b/simulator/fault_injection/CMakeLists.txt index 7cd78e35aadd2..0bc1663412960 100644 --- a/simulator/fault_injection/CMakeLists.txt +++ b/simulator/fault_injection/CMakeLists.txt @@ -15,7 +15,7 @@ rclcpp_components_register_node(fault_injection_node_component if(BUILD_TESTING) # gtest - ament_add_gtest(test_fault_injection_node_component + ament_add_ros_isolated_gtest(test_fault_injection_node_component test/src/main.cpp test/src/test_diagnostic_storage.cpp ) diff --git a/simulator/fault_injection/package.xml b/simulator/fault_injection/package.xml index 961a0af1e5854..95e50e6448807 100644 --- a/simulator/fault_injection/package.xml +++ b/simulator/fault_injection/package.xml @@ -19,7 +19,7 @@ tier4_autoware_utils tier4_simulation_msgs - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common launch diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index d62a84e7d7b6f..6034697304059 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -28,7 +28,7 @@ rclcpp_components_register_node(${PROJECT_NAME} ) if(BUILD_TESTING) - ament_add_gtest(test_simple_planning_simulator + ament_add_ros_isolated_gtest(test_simple_planning_simulator test/test_simple_planning_simulator.cpp TIMEOUT 120 ) diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 761aef0e322f2..2c63c861039ed 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -31,7 +31,7 @@ launch_ros - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml b/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml index 12b6efa79d1cd..ef594927dac4d 100644 --- a/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml +++ b/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml @@ -10,3 +10,4 @@ width_m: 2.0 front_overhang_m: 0.5 rear_overhang_m: 0.5 + max_steer_angle: 0.70 # [rad] diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index d983aeed4d8c4..fea6cd16db9d0 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -190,6 +190,7 @@ void declareVehicleInfoParams(rclcpp::NodeOptions & node_options) node_options.append_parameter_override("left_overhang", 0.5); node_options.append_parameter_override("right_overhang", 0.5); node_options.append_parameter_override("vehicle_height", 1.5); + node_options.append_parameter_override("max_steer_angle", 0.7); } // Send a control command and run the simulation. diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp index 3f5ff8d2c0773..99c3c365c04fb 100644 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp @@ -126,6 +126,11 @@ void AutowareStateMonitorNode::onOdometry(const nav_msgs::msg::Odometry::ConstSh state_input_.odometry_buffer.pop_front(); } + + constexpr size_t odometry_buffer_size = 200; // 40Hz * 5sec + if (state_input_.odometry_buffer.size() > odometry_buffer_size) { + state_input_.odometry_buffer.pop_front(); + } } bool AutowareStateMonitorNode::onShutdownService( diff --git a/system/system_monitor/CMakeLists.txt b/system/system_monitor/CMakeLists.txt index 017e08393baae..65e933d072046 100644 --- a/system/system_monitor/CMakeLists.txt +++ b/system/system_monitor/CMakeLists.txt @@ -180,7 +180,7 @@ rclcpp_components_register_node(gpu_monitor_lib # TODO(yunus.caliskan): Port the tests to ROS2, robustify the tests. if(BUILD_TESTING) - # ament_add_gtest(test_cpu_monitor + # ament_add_ros_isolated_gtest(test_cpu_monitor # test/src/cpu_monitor/test_${CMAKE_CPU_PLATFORM}_cpu_monitor.cpp # ${CPU_MONITOR_SOURCE} # ) @@ -196,7 +196,7 @@ if(BUILD_TESTING) # target_link_libraries(test_cpu_monitor ${Boost_LIBRARIES} ${LIBRARIES}) - # ament_add_gtest(test_hdd_monitor + # ament_add_ros_isolated_gtest(test_hdd_monitor # test/src/hdd_monitor/test_hdd_monitor.cpp # src/hdd_monitor/hdd_monitor.cpp # ) @@ -213,7 +213,7 @@ if(BUILD_TESTING) # target_link_libraries(test_hdd_monitor ${Boost_LIBRARIES} ${LIBRARIES} # ) - # ament_add_gtest(test_mem_monitor + # ament_add_ros_isolated_gtest(test_mem_monitor # test/src/mem_monitor/test_mem_monitor.cpp # src/mem_monitor/mem_monitor.cpp # ) @@ -229,7 +229,7 @@ if(BUILD_TESTING) # target_link_libraries(test_mem_monitor ${Boost_LIBRARIES} ${LIBRARIES}) - # ament_add_gtest(test_net_monitor + # ament_add_ros_isolated_gtest(test_net_monitor # test/src/net_monitor/test_net_monitor.cpp # src/net_monitor/net_monitor.cpp # src/net_monitor/nl80211.cpp @@ -246,7 +246,7 @@ if(BUILD_TESTING) # target_link_libraries(test_net_monitor ${Boost_LIBRARIES} ${NL_LIBS} ${LIBRARIES}) - # ament_add_gtest(test_ntp_monitor + # ament_add_ros_isolated_gtest(test_ntp_monitor # test/src/ntp_monitor/test_ntp_monitor.cpp # src/ntp_monitor/ntp_monitor.cpp # ) @@ -262,7 +262,7 @@ if(BUILD_TESTING) # target_link_libraries(test_ntp_monitor ${Boost_LIBRARIES} ${LIBRARIES}) - # ament_add_gtest(test_process_monitor + # ament_add_ros_isolated_gtest(test_process_monitor # test/src/process_monitor/test_process_monitor.cpp # src/process_monitor/process_monitor.cpp # ) @@ -278,7 +278,7 @@ if(BUILD_TESTING) # target_link_libraries(test_process_monitor ${Boost_LIBRARIES} ${LIBRARIES}) - # ament_add_gtest(test_gpu_monitor + # ament_add_ros_isolated_gtest(test_gpu_monitor # test/src/gpu_monitor/test_${CMAKE_GPU_PLATFORM}_gpu_monitor.cpp # ${GPU_MONITOR_SOURCE} # ) diff --git a/system/system_monitor/package.xml b/system/system_monitor/package.xml index 8cca9d2c71545..559dc9c072592 100644 --- a/system/system_monitor/package.xml +++ b/system/system_monitor/package.xml @@ -23,7 +23,7 @@ chrony sysstat - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/system/system_monitor/src/gpu_monitor/tegra_gpu_monitor.cpp b/system/system_monitor/src/gpu_monitor/tegra_gpu_monitor.cpp index a9ae72321efd1..8da0d071de582 100644 --- a/system/system_monitor/src/gpu_monitor/tegra_gpu_monitor.cpp +++ b/system/system_monitor/src/gpu_monitor/tegra_gpu_monitor.cpp @@ -127,8 +127,10 @@ void GPUMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) } } -void GPUMonitor::checkThrottling(diagnostic_updater::DiagnosticStatusWrapper & stat) +void GPUMonitor::checkThrottling( + [[maybe_unused]] diagnostic_updater::DiagnosticStatusWrapper & stat) { + // Please remove the [[maybe_unused]] tag after implementation, it's a temp build fix // TODO(Fumihito Ito): implement me } diff --git a/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt b/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt index d85b418d6fcbe..36b96a2a300c5 100644 --- a/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt +++ b/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt @@ -28,7 +28,7 @@ if(BUILD_TESTING) test/test_raw_vehicle_cmd_converter.cpp ) set(TEST_RAW_VEHICLE_CMD_CONVERTER_EXE test_raw_vehicle_cmd_converter) - ament_add_gtest(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} ${TEST_SOURCES}) + ament_add_ros_isolated_gtest(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} ${TEST_SOURCES}) target_link_libraries(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} actuation_map_converter raw_vehicle_cmd_converter_node_component) endif() diff --git a/vehicle/raw_vehicle_cmd_converter/package.xml b/vehicle/raw_vehicle_cmd_converter/package.xml index b14b00c9d3963..4f36c95e5d82d 100644 --- a/vehicle/raw_vehicle_cmd_converter/package.xml +++ b/vehicle/raw_vehicle_cmd_converter/package.xml @@ -26,7 +26,7 @@ rclpy - ament_cmake_gtest + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml b/vehicle/vehicle_info_util/config/vehicle_info.param.yaml index b1279b50ef04a..8941b92b4e78e 100644 --- a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml +++ b/vehicle/vehicle_info_util/config/vehicle_info.param.yaml @@ -9,3 +9,4 @@ left_overhang: 0.1 # between left wheel center and vehicle left right_overhang: 0.1 # between right wheel center and vehicle right vehicle_height: 2.5 + max_steer_angle: 0.70 # [rad] diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp b/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp index e7ba446051889..8df6d4cfe04c1 100644 --- a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp +++ b/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp @@ -31,6 +31,7 @@ struct VehicleInfo double left_overhang_m; double right_overhang_m; double vehicle_height_m; + double max_steer_angle_m; // Derived parameters, i.e. calculated from base parameters // The offset values are relative to the base frame origin, which is located @@ -49,7 +50,8 @@ struct VehicleInfo inline VehicleInfo createVehicleInfo( const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m, const double wheel_tread_m, const double front_overhang_m, const double rear_overhang_m, - const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m) + const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m, + const double max_steer_angle_m) { // Calculate derived parameters const double vehicle_length_m_ = front_overhang_m + wheel_base_m + rear_overhang_m; @@ -72,6 +74,7 @@ inline VehicleInfo createVehicleInfo( left_overhang_m, right_overhang_m, vehicle_height_m, + max_steer_angle_m, // Derived parameters vehicle_length_m_, vehicle_width_m_, diff --git a/vehicle/vehicle_info_util/src/vehicle_info_util.cpp b/vehicle/vehicle_info_util/src/vehicle_info_util.cpp index 6cc2d2f27e9ce..c295155727e61 100644 --- a/vehicle/vehicle_info_util/src/vehicle_info_util.cpp +++ b/vehicle/vehicle_info_util/src/vehicle_info_util.cpp @@ -49,6 +49,7 @@ VehicleInfoUtil::VehicleInfoUtil(rclcpp::Node & node) vehicle_info_.left_overhang_m = getParameter(node, "left_overhang"); vehicle_info_.right_overhang_m = getParameter(node, "right_overhang"); vehicle_info_.vehicle_height_m = getParameter(node, "vehicle_height"); + vehicle_info_.max_steer_angle_m = getParameter(node, "max_steer_angle"); } VehicleInfo VehicleInfoUtil::getVehicleInfo() @@ -56,7 +57,8 @@ VehicleInfo VehicleInfoUtil::getVehicleInfo() return createVehicleInfo( vehicle_info_.wheel_radius_m, vehicle_info_.wheel_width_m, vehicle_info_.wheel_base_m, vehicle_info_.wheel_tread_m, vehicle_info_.front_overhang_m, vehicle_info_.rear_overhang_m, - vehicle_info_.left_overhang_m, vehicle_info_.right_overhang_m, vehicle_info_.vehicle_height_m); + vehicle_info_.left_overhang_m, vehicle_info_.right_overhang_m, vehicle_info_.vehicle_height_m, + vehicle_info_.max_steer_angle_m); } } // namespace vehicle_info_util