diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions new file mode 100644 index 0000000000000..5b140152e7f08 --- /dev/null +++ b/.cppcheck_suppressions @@ -0,0 +1,59 @@ +arrayIndexThenCheck +assignBoolToFloat +checkersReport +constParameterPointer +constParameterReference +constStatement +constVariable +constVariablePointer +constVariableReference +containerOutOfBounds +cstyleCast +ctuOneDefinitionRuleViolation +current_deleted_index +duplicateAssignExpression +duplicateBranch +duplicateBreak +duplicateCondition +duplicateExpression +funcArgNamesDifferent +functionConst +functionStatic +invalidPointerCast +knownConditionTrueFalse +missingInclude +missingIncludeSystem +multiCondition +noConstructor +noExplicitConstructor +noValidConfiguration +obstacle_cruise_planner +passedByValue +preprocessorErrorDirective +redundantAssignment +redundantContinue +redundantIfRemove +redundantInitialization +returnByReference +selfAssignment +shadowArgument +shadowFunction +shadowVariable +stlFindInsert +syntaxError +uninitMemberVar +unknownMacro +unmatchedSuppression +unpreciseMathCall +unreadVariable +unsignedLessThanZero +unusedFunction +unusedScopedObject +unusedStructMember +unusedVariable +useInitializationList +useStlAlgorithm +uselessCallsSubstr +uselessOverride +variableScope +virtualCallInConstructor diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 390243aa2584f..da54ae2e768d3 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -147,6 +147,7 @@ perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp +planning/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @@ -174,7 +175,6 @@ planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp m planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp diff --git a/.github/workflows/cppcheck-all.yaml b/.github/workflows/cppcheck-all.yaml new file mode 100644 index 0000000000000..db3bd5d259895 --- /dev/null +++ b/.github/workflows/cppcheck-all.yaml @@ -0,0 +1,60 @@ +name: cppcheck-all + +on: + pull_request: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + cppcheck-all: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Run Cppcheck on all files + continue-on-error: true + id: cppcheck + run: | + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --xml . 2> cppcheck-report.xml + shell: bash + + - name: Count errors by error ID and severity + run: | + #!/bin/bash + temp_file=$(mktemp) + grep -oP '(?<=id=")[^"]+" severity="[^"]+' cppcheck-report.xml | sed 's/" severity="/,/g' > "$temp_file" + echo "Error counts by error ID and severity:" + sort "$temp_file" | uniq -c + rm "$temp_file" + shell: bash + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.xml + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml new file mode 100644 index 0000000000000..914abd7df86ea --- /dev/null +++ b/.github/workflows/cppcheck-differential.yaml @@ -0,0 +1,65 @@ +name: cppcheck-differential + +on: + pull_request: + +jobs: + cppcheck-differential: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Get changed files + id: changed-files + run: | + git fetch origin ${{ github.base_ref }} --depth=1 + git diff --name-only FETCH_HEAD ${{ github.sha }} > changed_files.txt + cat changed_files.txt + + - name: Run Cppcheck on changed files + continue-on-error: true + id: cppcheck + run: | + files=$(cat changed_files.txt | grep -E '\.(cpp|hpp)$' || true) + if [ -n "$files" ]; then + echo "Running Cppcheck on changed files: $files" + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions $files 2> cppcheck-report.txt + else + echo "No C++ files changed." + touch cppcheck-report.txt + fi + shell: bash + + - name: Show cppcheck-report result + run: | + cat cppcheck-report.txt + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.txt + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 3662179babffa..b093bc6295e65 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -136,7 +136,7 @@ /> - + diff --git a/localization/yabloc/yabloc_image_processing/CMakeLists.txt b/localization/yabloc/yabloc_image_processing/CMakeLists.txt index 58437c085c4b2..2af75ab237585 100644 --- a/localization/yabloc/yabloc_image_processing/CMakeLists.txt +++ b/localization/yabloc/yabloc_image_processing/CMakeLists.txt @@ -14,56 +14,55 @@ find_package(OpenCV REQUIRED) # PCL find_package(PCL REQUIRED COMPONENTS common) +ament_auto_add_library(${PROJECT_NAME} SHARED + src/line_segment_detector/line_segment_detector_core.cpp + src/graph_segment/graph_segment_core.cpp + src/graph_segment/similar_area_searcher.cpp + src/segment_filter/segment_filter_core.cpp + src/undistort/undistort_node.cpp + src/line_segments_overlay/line_segments_overlay_core.cpp + src/lanelet2_overlay/lanelet2_overlay_core.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ${OpenCV_LIBS}) + # =================================================== # Executable -# line segment detector -set(TARGET line_segment_detector_node) -ament_auto_add_executable(${TARGET} - src/line_segment_detector/line_segment_detector_node.cpp - src/line_segment_detector/line_segment_detector_core.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::graph_segment::GraphSegment" + EXECUTABLE yabloc_graph_segment_node + EXECUTOR SingleThreadedExecutor +) -# graph based segmentation -set(TARGET graph_segment_node) -ament_auto_add_executable(${TARGET} - src/graph_segment/graph_segment_node.cpp - src/graph_segment/graph_segment_core.cpp - src/graph_segment/similar_area_searcher.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::lanelet2_overlay::Lanelet2Overlay" + EXECUTABLE yabloc_lanelet2_overlay_node + EXECUTOR SingleThreadedExecutor +) -# segment filter -set(TARGET segment_filter_node) -ament_auto_add_executable(${TARGET} - src/segment_filter/segment_filter_node.cpp - src/segment_filter/segment_filter_core.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::line_segment_detector::LineSegmentDetector" + EXECUTABLE yabloc_line_segment_detector_node + EXECUTOR SingleThreadedExecutor +) -# undistort -set(TARGET undistort_node) -ament_auto_add_executable(${TARGET} - src/undistort/undistort_node.cpp) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::line_segments_overlay::LineSegmentsOverlay" + EXECUTABLE yabloc_line_segments_overlay_node + EXECUTOR SingleThreadedExecutor +) -# line_segments_overlay -set(TARGET line_segments_overlay_node) -ament_auto_add_executable(${TARGET} - src/line_segments_overlay/line_segments_overlay_core.cpp - src/line_segments_overlay/line_segments_overlay_node.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::segment_filter::SegmentFilter" + EXECUTABLE yabloc_segment_filter_node + EXECUTOR SingleThreadedExecutor +) -# lanelet2_overlay -set(TARGET lanelet2_overlay_node) -ament_auto_add_executable(${TARGET} - src/lanelet2_overlay/lanelet2_overlay_core.cpp - src/lanelet2_overlay/lanelet2_overlay_node.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::undistort::UndistortNode" + EXECUTABLE yabloc_undistort_node + EXECUTOR SingleThreadedExecutor +) # =================================================== ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp index a1e9c90eaa362..6c2e670a7a72e 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp @@ -32,7 +32,7 @@ class GraphSegment : public rclcpp::Node public: using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - GraphSegment(); + explicit GraphSegment(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const float target_height_ratio_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp index 468e765b74175..7d644376ba591 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp @@ -47,7 +47,7 @@ class Lanelet2Overlay : public rclcpp::Node using Image = sensor_msgs::msg::Image; using Float32Array = std_msgs::msg::Float32MultiArray; - Lanelet2Overlay(); + explicit Lanelet2Overlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: common::StaticTfSubscriber tf_subscriber_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp index 74ef82dd94f59..761c581200369 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp @@ -42,7 +42,7 @@ class LineSegmentDetector : public rclcpp::Node using Image = sensor_msgs::msg::Image; using PointCloud2 = sensor_msgs::msg::PointCloud2; - LineSegmentDetector(); + explicit LineSegmentDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: rclcpp::Subscription::SharedPtr sub_image_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp index abbc2f75725ed..00f01be5984e5 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp @@ -34,7 +34,7 @@ class LineSegmentsOverlay : public rclcpp::Node using Image = sensor_msgs::msg::Image; using LineSegment = pcl::PointXYZLNormal; using LineSegments = pcl::PointCloud; - LineSegmentsOverlay(); + explicit LineSegmentsOverlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: void on_image(const Image::ConstSharedPtr & img_msg); diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp index 766bb77d4da85..c8f036f2b12de 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp @@ -39,7 +39,7 @@ class SegmentFilter : public rclcpp::Node using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - SegmentFilter(); + explicit SegmentFilter(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: using ProjectFunc = std::function(const Eigen::Vector3f &)>; diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index a0cad16302c2b..214772751f931 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -5,7 +5,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -27,7 +27,7 @@ - + @@ -40,7 +40,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml index 150ffed58138d..134f3ee765476 100644 --- a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml @@ -10,7 +10,7 @@ - + @@ -23,7 +23,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 209f09fdaa7ac..416acfdc76a16 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -19,6 +19,7 @@ cv_bridge pcl_conversions rclcpp + rclcpp_components sensor_msgs std_msgs tier4_autoware_utils diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp index 75e14b5a4cd4b..f8d4c74dd5bf8 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::graph_segment { -GraphSegment::GraphSegment() -: Node("graph_segment"), +GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) +: Node("graph_segment", options), target_height_ratio_(declare_parameter("target_height_ratio")), target_candidate_box_width_(declare_parameter("target_candidate_box_width")) { @@ -159,3 +159,6 @@ void GraphSegment::draw_and_publish_image( } } // namespace yabloc::graph_segment + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::graph_segment::GraphSegment) diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp deleted file mode 100644 index d11701e115eff..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/graph_segment/graph_segment.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp index 367ff51567147..107d861364038 100644 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp @@ -28,8 +28,8 @@ namespace yabloc::lanelet2_overlay { -Lanelet2Overlay::Lanelet2Overlay() -: Node("lanelet2_overlay"), tf_subscriber_(get_clock()), pose_buffer_{40} +Lanelet2Overlay::Lanelet2Overlay(const rclcpp::NodeOptions & options) +: Node("lanelet2_overlay", options), tf_subscriber_(get_clock()), pose_buffer_{40} { using std::placeholders::_1; @@ -211,3 +211,6 @@ void Lanelet2Overlay::make_vis_marker( } } // namespace yabloc::lanelet2_overlay + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::lanelet2_overlay::Lanelet2Overlay) diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp deleted file mode 100644 index 941ac9d84ab16..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index 57f2302fd5c5a..c613642628499 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -23,7 +23,8 @@ namespace yabloc::line_segment_detector { -LineSegmentDetector::LineSegmentDetector() : Node("line_detector") +LineSegmentDetector::LineSegmentDetector(const rclcpp::NodeOptions & options) +: Node("line_detector", options) { using std::placeholders::_1; @@ -106,3 +107,6 @@ std::vector LineSegmentDetector::remove_too_outer_elements( } } // namespace yabloc::line_segment_detector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segment_detector::LineSegmentDetector) diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp deleted file mode 100644 index 42965ae853ea7..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/line_segment_detector/line_segment_detector.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp index 0ee4115a39760..7e06de81fbd18 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::line_segments_overlay { -LineSegmentsOverlay::LineSegmentsOverlay() -: Node("overlay_lanelet2"), +LineSegmentsOverlay::LineSegmentsOverlay(const rclcpp::NodeOptions & options) +: Node("line_segments_overlay", options), max_buffer_size_(static_cast(declare_parameter("max_buffer_size", 5))) { using std::placeholders::_1; @@ -90,3 +90,6 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l } } // namespace yabloc::line_segments_overlay + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segments_overlay::LineSegmentsOverlay) diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp deleted file mode 100644 index cac6a6a0c5a66..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp index cb1505903b210..df0aa7d65c617 100644 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::segment_filter { -SegmentFilter::SegmentFilter() -: Node("segment_filter"), +SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) +: Node("segment_filter", options), image_size_(declare_parameter("image_size")), max_range_(declare_parameter("max_range")), min_segment_length_(declare_parameter("min_segment_length")), @@ -282,3 +282,6 @@ std::set SegmentFilter::filter_by_mask( } } // namespace yabloc::segment_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::segment_filter::SegmentFilter) diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp deleted file mode 100644 index ea51babceee60..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/segment_filter/segment_filter.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 7fc9ad785dbe2..0714b6c8091c8 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -37,8 +37,8 @@ class UndistortNode : public rclcpp::Node using CameraInfo = sensor_msgs::msg::CameraInfo; using Image = sensor_msgs::msg::Image; - UndistortNode() - : Node("undistort"), + explicit UndistortNode(const rclcpp::NodeOptions & options) + : Node("undistort", options), OUTPUT_WIDTH(declare_parameter("width")), OVERRIDE_FRAME_ID(declare_parameter("override_frame_id")) { @@ -166,10 +166,5 @@ class UndistortNode : public rclcpp::Node }; } // namespace yabloc::undistort -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::undistort::UndistortNode) diff --git a/planning/.pages b/planning/.pages index 44bd36970674f..a11339d6f1df8 100644 --- a/planning/.pages +++ b/planning/.pages @@ -10,13 +10,13 @@ nav: - 'Dynamic Drivable Area': planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design - 'Turn Signal': planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design - 'Scene Module': - - 'Avoidance': planning/behavior_path_avoidance_module - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module - - 'Dynamic Avoidance': planning/behavior_path_dynamic_avoidance_module + - 'Dynamic Obstacle Avoidance': planning/autoware_behavior_path_dynamic_obstacle_avoidance_module - 'Goal Planner': planning/behavior_path_goal_planner_module - 'Lane Change': planning/behavior_path_lane_change_module - 'Side Shift': planning/behavior_path_side_shift_module - 'Start Planner': planning/behavior_path_start_planner_module + - 'Static Obstacle Avoidance': planning/autoware_behavior_path_static_obstacle_avoidance_module - 'Behavior Velocity Planner': - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner - 'Template for Custom Module': planning/autoware_behavior_velocity_template_module @@ -34,7 +34,7 @@ nav: - 'Speed Bump': planning/behavior_velocity_speed_bump_module - 'Stop Line': planning/behavior_velocity_stop_line_module - 'Traffic Light': planning/behavior_velocity_traffic_light_module - - 'Virtual Traffic Light': planning/behavior_velocity_virtual_traffic_light_module + - 'Virtual Traffic Light': planning/autoware_behavior_velocity_virtual_traffic_light_module - 'Walkway': planning/behavior_velocity_walkway_module - 'Parking': - 'Freespace Planner': @@ -67,9 +67,9 @@ nav: - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ - 'Available Modules': - 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/ - - 'Motion Velocity Smoother': - - 'About Motion Velocity Smoother': planning/motion_velocity_smoother - - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja + - 'Velocity Smoother': + - 'About Velocity Smoother': planning/autoware_velocity_smoother + - 'About Velocity Smoother (Japanese)': planning/autoware_velocity_smoother/README.ja - 'Scenario Selector': planning/scenario_selector - 'Static Centerline Generator': planning/autoware_static_centerline_generator - 'API and Library': @@ -79,6 +79,7 @@ nav: - 'Route Handler': planning/route_handler - 'RTC Interface': planning/rtc_interface - 'Additional Tools': + - 'Remaining Distance Time Calculator': planning/autoware_remaining_distance_time_calculator - 'RTC Replayer': planning/rtc_replayer - 'Planning Debug Tools': - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md index 0b29d463bb105..94784fe6771c0 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md @@ -9,8 +9,8 @@ Each module performs the following roles. Dynamic Avoidance module cuts off the drivable area according to the position and velocity of the target to be avoided. Obstacle Avoidance module modifies the path to be followed so that it fits within the received drivable area. -Avoidance functions are also provided by the [Avoidance module](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_avoidance_module/), but these modules have different roles. -The Avoidance module performs avoidance through the outside of own lanes but cannot avoid the moving objects. +Static obstacle's avoidance functions are also provided by the [Static Avoidance module](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_behavior_path_static_obstacle_avoidance_module/), but these modules have different roles. +The Static Obstacle Avoidance module performs avoidance through the outside of own lanes but cannot avoid the moving objects. On the other hand, this module can avoid moving objects. For this reason, the word "dynamic" is used in the module's name. The table below lists the avoidance modules that can handle each situation. diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index e0f1156172932..cb150514ac372 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -1,9 +1,9 @@ { "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for behavior_path_avoidance_module", + "title": "Parameters for behavior_path_static_obstacle_avoidance_module", "type": "object", "definitions": { - "behavior_path_avoidance_module": { + "behavior_path_static_obstacle_avoidance_module": { "type": "object", "properties": { "resample_interval_for_planning": { @@ -1473,7 +1473,7 @@ "type": "object", "properties": { "avoidance": { - "$ref": "#/definitions/behavior_path_avoidance_module" + "$ref": "#/definitions/behavior_path_static_obstacle_avoidance_module" } }, "required": ["avoidance"], diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 0a6d490f7fbf1..48dae73231b9f 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -867,10 +867,12 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() if (data.state == AvoidanceState::SUCCEEDED) { removeRegisteredShiftLines(State::SUCCEEDED); + return getPreviousModuleOutput(); } if (data.state == AvoidanceState::CANCEL) { removeRegisteredShiftLines(State::FAILED); + return getPreviousModuleOutput(); } if (data.yield_required) { @@ -915,11 +917,21 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt; }; + const auto is_large_deviation = [this](const auto & path) { + constexpr double threshold = 1.0; + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); + const auto lateral_deviation = + motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); + return std::abs(lateral_deviation) > threshold; + }; + // turn signal info if (path_shifter_.getShiftLines().empty()) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else if (is_large_deviation(spline_shift_path.path)) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; @@ -930,7 +942,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/autoware_behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md index 47db8f3bedfc9..bcef46e90c1ec 100644 --- a/planning/autoware_behavior_velocity_planner/README.md +++ b/planning/autoware_behavior_velocity_planner/README.md @@ -14,7 +14,7 @@ It loads modules as plugins. Please refer to the links listed below for detail o - [Intersection](../behavior_velocity_intersection_module/README.md) - [MergeFromPrivate](../behavior_velocity_intersection_module/README.md#merge-from-private) - [Stop Line](../behavior_velocity_stop_line_module/README.md) -- [Virtual Traffic Light](../behavior_velocity_virtual_traffic_light_module/README.md) +- [Virtual Traffic Light](../autoware_behavior_velocity_virtual_traffic_light_module/README.md) - [Traffic Light](../behavior_velocity_traffic_light_module/README.md) - [Occlusion Spot](../behavior_velocity_occlusion_spot_module/README.md) - [No Stopping Area](../behavior_velocity_no_stopping_area_module/README.md) diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index fd513f2687173..fff1fb8ff91c0 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -66,6 +66,7 @@ ament_cmake_ros ament_lint_auto autoware_behavior_velocity_run_out_module + autoware_behavior_velocity_virtual_traffic_light_module autoware_lint_common behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module @@ -78,7 +79,6 @@ behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_traffic_light_module - behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index fb7287314f859..bc60df53df6c5 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -75,7 +75,7 @@ std::shared_ptr generateNode() module_names.emplace_back("behavior_velocity_planner::MergeFromPrivateModulePlugin"); module_names.emplace_back("behavior_velocity_planner::BlindSpotModulePlugin"); module_names.emplace_back("behavior_velocity_planner::DetectionAreaModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::VirtualTrafficLightModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); module_names.emplace_back("behavior_velocity_planner::NoStoppingAreaModulePlugin"); module_names.emplace_back("behavior_velocity_planner::StopLineModulePlugin"); module_names.emplace_back("behavior_velocity_planner::OcclusionSpotModulePlugin"); @@ -103,11 +103,13 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config_no_prefix("intersection"), get_behavior_velocity_module_config_no_prefix("no_stopping_area"), get_behavior_velocity_module_config_no_prefix("occlusion_spot"), + get_behavior_velocity_module_config("run_out"), get_behavior_velocity_module_config_no_prefix("speed_bump"), get_behavior_velocity_module_config_no_prefix("stop_line"), get_behavior_velocity_module_config_no_prefix("traffic_light"), - get_behavior_velocity_module_config_no_prefix("virtual_traffic_light"), + get_behavior_velocity_module_config("virtual_traffic_light"), + get_behavior_velocity_module_config_no_prefix("out_of_lane"), get_behavior_velocity_module_config_no_prefix("no_drivable_lane")}); diff --git a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt b/planning/autoware_behavior_velocity_virtual_traffic_light_module/CMakeLists.txt similarity index 83% rename from planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/CMakeLists.txt index a1c00cf49db29..9d1fe762262dd 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_virtual_traffic_light_module) +project(autoware_behavior_velocity_virtual_traffic_light_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_virtual_traffic_light_module/README.md b/planning/autoware_behavior_velocity_virtual_traffic_light_module/README.md similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/README.md rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/README.md diff --git a/planning/behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/restart.svg b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart.svg similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/restart.svg rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart.svg diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg diff --git a/planning/behavior_velocity_virtual_traffic_light_module/package.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml similarity index 89% rename from planning/behavior_velocity_virtual_traffic_light_module/package.xml rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 2cf3847c66935..cd35d4308c26a 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_virtual_traffic_light_module + autoware_behavior_velocity_virtual_traffic_light_module 0.1.0 - The behavior_velocity_virtual_traffic_light_module package + The autoware_behavior_velocity_virtual_traffic_light_module package Kosuke Takeuchi Tomoya Kimura diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml new file mode 100644 index 0000000000000..2402fc13469b9 --- /dev/null +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 83b5e6317aeb0..2f389d5d1104f 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -28,7 +28,7 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::toMsg; using namespace std::literals::string_literals; -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -147,4 +147,4 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createDebugMarke return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp similarity index 92% rename from planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index cd8047cff24e0..5a32cfd2f74f0 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -24,10 +24,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::VirtualTrafficLight; using tier4_autoware_utils::getOrDeclareParameter; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -76,9 +77,9 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( return id_set.count(scene_module->getModuleId()) == 0; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::VirtualTrafficLightModulePlugin, + autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp similarity index 86% rename from planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index fa41e50d96e3f..c73bb0d706008 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -27,8 +27,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PluginWrapper; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::SceneModuleManagerInterface; class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface { public: @@ -48,6 +51,6 @@ class VirtualTrafficLightModulePlugin : public PluginWrapper #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PlanningBehavior; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::VelocityFactor; +namespace arc_lane_utils = ::behavior_velocity_planner::arc_lane_utils; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace { using tier4_autoware_utils::calcDistance2d; @@ -618,4 +623,4 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( module_data_.stop_head_pose_at_end_line = calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp similarity index 93% rename from planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 7715bb127962f..c83ff4e0607ef 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -30,8 +30,12 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PathWithLaneId; +using ::behavior_velocity_planner::Pose; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::StopReason; class VirtualTrafficLightModule : public SceneModuleInterface { public: @@ -126,5 +130,5 @@ class VirtualTrafficLightModule : public SceneModuleInterface tier4_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/README.md b/planning/behavior_path_avoidance_by_lane_change_module/README.md index d91d7116ee056..036de718ccde8 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/README.md +++ b/planning/behavior_path_avoidance_by_lane_change_module/README.md @@ -13,7 +13,7 @@ This module is designed as one of the obstacle avoidance features and generates ## Inner-workings / Algorithms -Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Normal Avoidance Module](../behavior_path_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. +Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Static Object Avoidance Module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. Check that the following conditions are satisfied after the filtering process for the avoidance target. diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 9bfef90668e8e..4acc4dce743ad 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1092,7 +1092,7 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const auto original_signal = getPreviousModuleOutput().turn_signal_info; const auto new_signal = calcTurnSignalInfo(); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index 9cbd864be9dbb..09603bedf27df 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -296,7 +296,7 @@ First, we divide the target objects into obstacles in the target lane, obstacles ![object lanes](./images/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_avoidance_module/README.md). +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md). The detection area for the target lane can be expanded beyond its original boundaries to enable detection of objects that are outside the target lane's limits. diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2d88a820e0fae..e1c4289b57612 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -204,7 +204,7 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() } // check the priority of turn signals - return getTurnSignalDecider().use_prior_turn_signal( + return getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); } @@ -226,7 +226,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const output.path = abort_path_->path; extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -252,7 +252,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -297,7 +297,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index da39bf0d0d0ba..7e335b09e4fd3 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -64,6 +64,17 @@ struct TurnSignalInfo hazard_signal.command = HazardLightsCommand::NO_COMMAND; } + TurnSignalInfo(const Pose & start, const Pose & end) + { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + hazard_signal.command = HazardLightsCommand::NO_COMMAND; + + desired_start_point = start; + desired_end_point = end; + required_start_point = start; + required_end_point = end; + } + // desired turn signal TurnIndicatorsCommand turn_signal; HazardLightsCommand hazard_signal; @@ -93,6 +104,11 @@ class TurnSignalDecider const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info, const double nearest_dist_threshold, const double nearest_yaw_threshold); + TurnSignalInfo overwrite_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const; + TurnSignalInfo use_prior_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index fb15391550980..146fb03cc00f2 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -392,6 +392,40 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( return intersection_signal_info.turn_signal; } +TurnSignalInfo TurnSignalDecider::overwrite_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const +{ + if (original_signal.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND) { + return new_signal; + } + + if (original_signal.turn_signal.command == TurnIndicatorsCommand::DISABLE) { + return new_signal; + } + + const auto get_distance = [&](const Pose & input_point) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point.position, + nearest_seg_idx) - + base_link2front_; + }; + + const auto & original_desired_end_point = original_signal.desired_end_point; + const auto & new_desired_start_point = new_signal.desired_start_point; + + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_new_desired_start = get_distance(new_desired_start_point); + if (dist_to_new_desired_start > dist_to_original_desired_end) { + return original_signal; + } + + return new_signal; +} + TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, @@ -615,6 +649,8 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check, const bool is_pull_out) const { + using tier4_autoware_utils::getPose; + const auto & p = parameters; const auto & rh = route_handler; const auto & ego_pose = self_odometry->pose.pose; @@ -667,16 +703,19 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const auto relative_shift_length = end_shift_length - start_shift_length; + const auto p_path_start = getPose(path.path.points.front()); + const auto p_path_end = getPose(path.path.points.back()); + // If shift length is shorter than the threshold, it does not need to turn on blinkers if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // If the vehicle does not shift anymore, we turn off the blinker if ( std::fabs(end_shift_length - current_shift_length) < p.turn_signal_remaining_shift_length_threshold) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } const auto get_command = [](const auto & shift_length) { @@ -691,7 +730,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( p.vehicle_info.max_longitudinal_offset_m; if (signal_prepare_distance < ego_front_to_shift_start) { - return std::make_pair(TurnSignalInfo{}, false); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false); } const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; @@ -708,13 +747,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( turn_signal_info.turn_signal.command = get_command(relative_shift_length); if (!p.turn_signal_on_swerving) { - return std::make_pair(turn_signal_info, false); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false); } lanelet::ConstLanelet lanelet; const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start; if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); @@ -729,13 +768,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( !is_pull_out && !existShiftSideLane( start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // Check if the ego will cross lane bounds. // Note that pull out requires blinkers, even if the ego does not cross lane bounds if (!is_pull_out && !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // If the ego has stopped and its close to completing its shift, turn off the blinkers @@ -744,7 +783,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( if (isNearEndOfShift( start_shift_length, end_shift_length, ego_pose.position, current_lanelets, p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } } diff --git a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml deleted file mode 100644 index 943ef175aa8f8..0000000000000 --- a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - -