From 481b54382ef306ea2483cac902fd60fdb0f55ba2 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Sat, 12 Mar 2022 13:34:03 +0900 Subject: [PATCH 01/15] chore: sync files (#511) Signed-off-by: GitHub Co-authored-by: kenji-miyake --- CODE_OF_CONDUCT.md | 132 +++++++++++++++++++++++++++++++++++++++++++++ CONTRIBUTING.md | 3 ++ DISCLAIMER.md | 46 ++++++++++++++++ 3 files changed, 181 insertions(+) create mode 100644 CODE_OF_CONDUCT.md create mode 100644 CONTRIBUTING.md create mode 100644 DISCLAIMER.md diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 000000000000..c493cad4da59 --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,132 @@ +# Contributor Covenant Code of Conduct + +## Our Pledge + +We as members, contributors, and leaders pledge to make participation in our +community a harassment-free experience for everyone, regardless of age, body +size, visible or invisible disability, ethnicity, sex characteristics, gender +identity and expression, level of experience, education, socio-economic status, +nationality, personal appearance, race, caste, color, religion, or sexual +identity and orientation. + +We pledge to act and interact in ways that contribute to an open, welcoming, +diverse, inclusive, and healthy community. + +## Our Standards + +Examples of behavior that contributes to a positive environment for our +community include: + +- Demonstrating empathy and kindness toward other people +- Being respectful of differing opinions, viewpoints, and experiences +- Giving and gracefully accepting constructive feedback +- Accepting responsibility and apologizing to those affected by our mistakes, + and learning from the experience +- Focusing on what is best not just for us as individuals, but for the overall + community + +Examples of unacceptable behavior include: + +- The use of sexualized language or imagery, and sexual attention or advances of + any kind +- Trolling, insulting or derogatory comments, and personal or political attacks +- Public or private harassment +- Publishing others' private information, such as a physical or email address, + without their explicit permission +- Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Enforcement Responsibilities + +Community leaders are responsible for clarifying and enforcing our standards of +acceptable behavior and will take appropriate and fair corrective action in +response to any behavior that they deem inappropriate, threatening, offensive, +or harmful. + +Community leaders have the right and responsibility to remove, edit, or reject +comments, commits, code, wiki edits, issues, and other contributions that are +not aligned to this Code of Conduct, and will communicate reasons for moderation +decisions when appropriate. + +## Scope + +This Code of Conduct applies within all community spaces, and also applies when +an individual is officially representing the community in public spaces. +Examples of representing our community include using an official e-mail address, +posting via an official social media account, or acting as an appointed +representative at an online or offline event. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported to the community leaders responsible for enforcement at +conduct@autoware.org. +All complaints will be reviewed and investigated promptly and fairly. + +All community leaders are obligated to respect the privacy and security of the +reporter of any incident. + +## Enforcement Guidelines + +Community leaders will follow these Community Impact Guidelines in determining +the consequences for any action they deem in violation of this Code of Conduct: + +### 1. Correction + +**Community Impact**: Use of inappropriate language or other behavior deemed +unprofessional or unwelcome in the community. + +**Consequence**: A private, written warning from community leaders, providing +clarity around the nature of the violation and an explanation of why the +behavior was inappropriate. A public apology may be requested. + +### 2. Warning + +**Community Impact**: A violation through a single incident or series of +actions. + +**Consequence**: A warning with consequences for continued behavior. No +interaction with the people involved, including unsolicited interaction with +those enforcing the Code of Conduct, for a specified period of time. This +includes avoiding interactions in community spaces as well as external channels +like social media. Violating these terms may lead to a temporary or permanent +ban. + +### 3. Temporary Ban + +**Community Impact**: A serious violation of community standards, including +sustained inappropriate behavior. + +**Consequence**: A temporary ban from any sort of interaction or public +communication with the community for a specified period of time. No public or +private interaction with the people involved, including unsolicited interaction +with those enforcing the Code of Conduct, is allowed during this period. +Violating these terms may lead to a permanent ban. + +### 4. Permanent Ban + +**Community Impact**: Demonstrating a pattern of violation of community +standards, including sustained inappropriate behavior, harassment of an +individual, or aggression toward or disparagement of classes of individuals. + +**Consequence**: A permanent ban from any sort of public interaction within the +community. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], +version 2.1, available at +[https://www.contributor-covenant.org/version/2/1/code_of_conduct.html][v2.1]. + +Community Impact Guidelines were inspired by +[Mozilla's code of conduct enforcement ladder][mozilla coc]. + +For answers to common questions about this code of conduct, see the FAQ at +[https://www.contributor-covenant.org/faq][faq]. Translations are available at +[https://www.contributor-covenant.org/translations][translations]. + +[homepage]: https://www.contributor-covenant.org +[v2.1]: https://www.contributor-covenant.org/version/2/1/code_of_conduct.html +[mozilla coc]: https://github.com/mozilla/diversity +[faq]: https://www.contributor-covenant.org/faq +[translations]: https://www.contributor-covenant.org/translations diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 000000000000..22c7ee28e8d9 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,3 @@ +# Contributing + +See . diff --git a/DISCLAIMER.md b/DISCLAIMER.md new file mode 100644 index 000000000000..1b5a9bbe4d98 --- /dev/null +++ b/DISCLAIMER.md @@ -0,0 +1,46 @@ +DISCLAIMER + +“Autoware” will be provided by The Autoware Foundation under the Apache License 2.0. +This “DISCLAIMER” will be applied to all users of Autoware (a “User” or “Users”) with +the Apache License 2.0 and Users shall hereby approve and acknowledge all the contents +specified in this disclaimer below and will be deemed to consent to this +disclaimer without any objection upon utilizing or downloading Autoware. + +Disclaimer and Waiver of Warranties + +1. AUTOWARE FOUNDATION MAKES NO REPRESENTATION OR WARRANTY OF ANY KIND, + EXPRESS OR IMPLIED, WITH RESPECT TO PROVIDING AUTOWARE (the “Service”) + including but not limited to any representation or warranty (i) of fitness or + suitability for a particular purpose contemplated by the Users, (ii) of the + expected functions, commercial value, accuracy, or usefulness of the Service, + (iii) that the use by the Users of the Service complies with the laws and + regulations applicable to the Users or any internal rules established by + industrial organizations, (iv) that the Service will be free of interruption or + defects, (v) of the non-infringement of any third party's right and (vi) the + accuracy of the content of the Services and the software itself. + +2. The Autoware Foundation shall not be liable for any damage incurred by the + User that are attributable to the Autoware Foundation for any reasons + whatsoever. UNDER NO CIRCUMSTANCES SHALL THE AUTOWARE FOUNDATION BE LIABLE FOR + INCIDENTAL, INDIRECT, SPECIAL OR FUTURE DAMAGES OR LOSS OF PROFITS. + +3. A User shall be entirely responsible for the content posted by the User and + its use of any content of the Service or the Website. If the User is held + responsible in a civil action such as a claim for damages or even in a criminal + case, the Autoware Foundation and member companies, governments and academic & + non-profit organizations and their directors, officers, employees and agents + (collectively, the “Indemnified Parties”) shall be completely discharged from + any rights or assertions the User may have against the Indemnified Parties, or + from any legal action, litigation or similar procedures. + +Indemnity + +A User shall indemnify and hold the Indemnified Parties harmless from any of +their damages, losses, liabilities, costs or expenses (including attorneys' +fees or criminal compensation), or any claims or demands made against the +Indemnified Parties by any third party, due to or arising out of, or in +connection with utilizing Autoware (including the representations and +warranties), the violation of applicable Product Liability Law of each country +(including criminal case) or violation of any applicable laws by the Users, or +the content posted by the User or its use of any content of the Service or the +Website. From 7aa71c009bd2819972a061e9bc6b88e7de23493c Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Sun, 13 Mar 2022 14:44:21 +0900 Subject: [PATCH 02/15] chore: add NOTICE (#512) Signed-off-by: Kenji Miyake --- NOTICE | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 NOTICE diff --git a/NOTICE b/NOTICE new file mode 100644 index 000000000000..981e257f9bd2 --- /dev/null +++ b/NOTICE @@ -0,0 +1,8 @@ +autowarefoundation/autoware.universe +Copyright 2021 The Autoware Foundation + +This product includes software developed at +The Autoware Foundation (https://www.autoware.org/). + +This product includes code developed by TIER IV. +Copyright 2017 TIER IV, Inc. From a03ab1f7a17a4244c50f16cc5e7672af87cdc01d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Sun, 13 Mar 2022 16:55:16 +0900 Subject: [PATCH 03/15] refactor(avoidance_module): change implementation to lambda (#486) * refactor(avoidance_module): change implementation to lambda In the generateExtendedDrivableArea function, the implementation to extend drivable areas with reference to object is refactor to lambda. This will allow the function to be used for some future planned updates. The refactor doesn't affect the behavior of the avoidance_module. Signed-off-by: Muhammad Zulfaqar Azmi * refactor: add default flag to extend_to_opposite side By default, the parameter is true, therefore it doesn't affect the original behavior. Signed-off-by: Muhammad Zulfaqar Azmi * ci(pre-commit): autofix * fix: rework function for better extension Signed-off-by: Muhammad Zulfaqar Azmi * refactor(route handler, avoidance_module): move the implementation The implementation is made into function and moved into route handler Signed-off-by: Muhammad Zulfaqar Azmi * Refactor: convert lambdas to function Signed-off-by: Muhammad Zulfaqar Azmi * chore: add comments for comments for documentation in the header file Signed-off-by: Muhammad Zulfaqar Azmi * Refactor: local variable rename Signed-off-by: Muhammad Zulfaqar Azmi * refactor: make a wrapper function to get furthest linestrings Also modified some avoidance_module to include the refactored functions Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../avoidance/avoidance_module.cpp | 76 +++++---------- .../include/route_handler/route_handler.hpp | 43 +++++++++ planning/route_handler/src/route_handler.cpp | 92 +++++++++++++++++++ 3 files changed, 158 insertions(+), 53 deletions(-) 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 9bb114b2b578..d9239591149d 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 @@ -241,38 +241,23 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( lanelet::BasicPoint3d overhang_basic_pose( object_data.overhang_pose.position.x, object_data.overhang_pose.position.y, object_data.overhang_pose.position.z); + const bool get_left = + isOnRight(object_data) && parameters_.enable_avoidance_over_same_direction; + const bool get_right = + !isOnRight(object_data) && parameters_.enable_avoidance_over_same_direction; + + const auto target_lines = rh->getFurthestLinestring( + overhang_lanelet, get_right, get_left, + parameters_.enable_avoidance_over_opposite_direction); + if (isOnRight(object_data)) { - const auto & target_left_line = [this, &rh, &overhang_lanelet]() { - if ( - parameters_.enable_avoidance_over_same_direction && - parameters_.enable_avoidance_over_opposite_direction) { - return rh->getLeftMostLinestring(overhang_lanelet); - } else if ( - parameters_.enable_avoidance_over_same_direction && - !parameters_.enable_avoidance_over_opposite_direction) { - return rh->getLeftMostSameDirectionLinestring(overhang_lanelet); - } - return overhang_lanelet.leftBound(); - }(); object_data.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_left_line.basicLineString())); - debug_linestring.push_back(target_left_line); + distance2d(to2D(overhang_basic_pose), to2D(target_lines.back().basicLineString())); + debug_linestring.push_back(target_lines.back()); } else { - const auto & target_right_line = [this, &rh, &overhang_lanelet]() { - if ( - parameters_.enable_avoidance_over_same_direction && - parameters_.enable_avoidance_over_opposite_direction) { - return rh->getRightMostLinestring(overhang_lanelet); - } else if ( - parameters_.enable_avoidance_over_same_direction && - !parameters_.enable_avoidance_over_opposite_direction) { - return rh->getRightMostSameDirectionLinestring(overhang_lanelet); - } - return overhang_lanelet.rightBound(); - }(); object_data.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_right_line.basicLineString())); - debug_linestring.push_back(target_right_line); + distance2d(to2D(overhang_basic_pose), to2D(target_lines.front().basicLineString())); + debug_linestring.push_back(target_lines.front()); } } @@ -1675,35 +1660,20 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c { // 0. Extend to right/left of objects for (const auto & obstacle : avoidance_data_.objects) { + lanelet::ConstLanelets search_lanelets; auto object_lanelet = obstacle.overhang_lanelet; + constexpr bool get_right = true; + constexpr bool get_left = true; + const bool include_opposite = parameters_.enable_avoidance_over_opposite_direction; if (isOnRight(obstacle)) { - auto lanelet_at_left = route_handler->getLeftLanelet(object_lanelet); - while (lanelet_at_left) { - extended_lanelets.push_back(lanelet_at_left.get()); - lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); - } - if (lanelet_at_left) { - auto lanelet_at_right = - planner_data_->route_handler->getRightLanelet(lanelet_at_left.get()); - while (lanelet_at_right) { - extended_lanelets.push_back(lanelet_at_right.get()); - lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); - } - } + search_lanelets = route_handler->getAllSharedLineStringLanelets( + object_lanelet, !get_right, get_left, include_opposite); } else { - auto lanelet_at_right = route_handler->getRightLanelet(object_lanelet); - while (lanelet_at_right) { - extended_lanelets.push_back(lanelet_at_right.get()); - lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); - } - if (lanelet_at_right) { - auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get()); - while (lanelet_at_left) { - extended_lanelets.push_back(lanelet_at_left.get()); - lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); - } - } + search_lanelets = route_handler->getAllSharedLineStringLanelets( + object_lanelet, get_right, !get_left, include_opposite); } + extended_lanelets.insert( + extended_lanelets.end(), search_lanelets.begin(), search_lanelets.end()); } } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 7cf38a40d0ee..8b6f35b86b5a 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -138,6 +138,36 @@ class RouteHandler */ lanelet::Lanelets getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const; + /** + * @brief Searches and return all lanelet on the left that shares same linestring + * @param the lanelet of interest + * @param (optional) flag to include the lane with opposite direction + * @return vector of lanelet that is connected via share linestring + */ + lanelet::ConstLanelets getAllLeftSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept; + + /** + * @brief Searches and return all lanelet on the right that shares same linestring + * @param the lanelet of interest + * @param (optional) flag to include the lane with opposite direction + * @return vector of lanelet that is connected via share linestring + */ + lanelet::ConstLanelets getAllRightSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept; + + /** + * @brief Searches and return all lanelet (left and right) that shares same linestring + * @param the lanelet of interest + * @param (optional) flag to search only right side + * @param (optional) flag to search only left side + * @param (optional) flag to include the lane with opposite direction + * @return vector of lanelet that is connected via share linestring + */ + lanelet::ConstLanelets getAllSharedLineStringLanelets( + const lanelet::ConstLanelet & current_lane, bool is_right = true, bool is_left = true, + bool is_opposite = true) const noexcept; + /** * @brief Searches the furthest linestring to the right side of the lanelet * Only lanelet with same direction is considered @@ -173,6 +203,19 @@ class RouteHandler */ lanelet::ConstLineString3d getLeftMostLinestring( const lanelet::ConstLanelet & lanelet) const noexcept; + + /** + * @brief Return furthest linestring on both side of the lanelet + * @param the lanelet of interest + * @param (optional) search furthest right side + * @param (optional) search furthest left side + * @param (optional) include opposite lane as well + * @return right and left linestrings + */ + lanelet::ConstLineStrings3d getFurthestLinestring( + const lanelet::ConstLanelet & lanelet, bool is_right = true, bool is_left = true, + bool is_opposite = true) const noexcept; + int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const; bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index ac48a20ea020..406e070f31c4 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -911,6 +911,73 @@ lanelet::Lanelets RouteHandler::getRightOppositeLanelets( return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert()); } +lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept +{ + lanelet::ConstLanelets linestring_shared; + auto lanelet_at_left = getLeftLanelet(lane); + auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane); + while (lanelet_at_left) { + linestring_shared.push_back(lanelet_at_left.get()); + lanelet_at_left = getLeftLanelet(lanelet_at_left.get()); + lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.get()); + } + + if (!lanelet_at_left_opposite.empty() && include_opposite) { + linestring_shared.push_back(lanelet_at_left_opposite.front()); + auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); + while (lanelet_at_right) { + linestring_shared.push_back(lanelet_at_right.get()); + lanelet_at_right = getRightLanelet(lanelet_at_right.get()); + } + } + return linestring_shared; +} + +lanelet::ConstLanelets RouteHandler::getAllRightSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept +{ + lanelet::ConstLanelets linestring_shared; + auto lanelet_at_right = getRightLanelet(lane); + auto lanelet_at_right_opposite = getRightOppositeLanelets(lane); + while (lanelet_at_right) { + linestring_shared.push_back(lanelet_at_right.get()); + lanelet_at_right = getRightLanelet(lanelet_at_right.get()); + lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.get()); + } + + if (!lanelet_at_right_opposite.empty() && include_opposite) { + linestring_shared.push_back(lanelet_at_right_opposite.front()); + auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); + while (lanelet_at_left) { + linestring_shared.push_back(lanelet_at_left.get()); + lanelet_at_left = getLeftLanelet(lanelet_at_left.get()); + } + } + return linestring_shared; +} + +lanelet::ConstLanelets RouteHandler::getAllSharedLineStringLanelets( + const lanelet::ConstLanelet & current_lane, bool is_right, bool is_left, + bool is_opposite) const noexcept +{ + lanelet::ConstLanelets shared{current_lane}; + + if (is_right) { + const lanelet::ConstLanelets all_right_lanelets = + getAllRightSharedLinestringLanelets(current_lane, is_opposite); + shared.insert(shared.end(), all_right_lanelets.begin(), all_right_lanelets.end()); + } + + if (is_left) { + const lanelet::ConstLanelets all_left_lanelets = + getAllLeftSharedLinestringLanelets(current_lane, is_opposite); + shared.insert(shared.end(), all_left_lanelets.begin(), all_left_lanelets.end()); + } + + return shared; +} + lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const { return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound().invert()); @@ -972,6 +1039,31 @@ lanelet::ConstLineString3d RouteHandler::getLeftMostLinestring( return {}; } +lanelet::ConstLineStrings3d RouteHandler::getFurthestLinestring( + const lanelet::ConstLanelet & lanelet, bool is_right, bool is_left, + bool is_opposite) const noexcept +{ + lanelet::ConstLineStrings3d linestrings; + linestrings.reserve(2); + + if (is_right && is_opposite) { + linestrings.emplace_back(getRightMostLinestring(lanelet)); + } else if (is_right && !is_opposite) { + linestrings.emplace_back(getRightMostSameDirectionLinestring(lanelet)); + } else { + linestrings.emplace_back(lanelet.rightBound()); + } + + if (is_left && is_opposite) { + linestrings.emplace_back(getLeftMostLinestring(lanelet)); + } else if (is_left && !is_opposite) { + linestrings.emplace_back(getLeftMostSameDirectionLinestring(lanelet)); + } else { + linestrings.emplace_back(lanelet.leftBound()); + } + return linestrings; +} + bool RouteHandler::getLaneChangeTarget( const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const { From 41c9d8e05369d9a7909d61f22ab407bf1001130f Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Mon, 14 Mar 2022 14:38:14 +0900 Subject: [PATCH 04/15] fix: set callback group to timer (#516) * fix(ad_service_state_monitor): fix callback group Signed-off-by: Shinnosuke Hirakawa * fix(external_cmd_selector): fix callback grouop Signed-off-by: Shinnosuke Hirakawa * chore: clang-format Signed-off-by: Shinnosuke Hirakawa --- .../src/external_cmd_selector/external_cmd_selector_node.cpp | 3 ++- .../ad_service_state_monitor_node.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp index 69e41f11e628..a93f1b4dc655 100644 --- a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp +++ b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp @@ -103,7 +103,8 @@ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_option // Timer const auto period_ns = rclcpp::Rate(update_rate).period(); timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&ExternalCmdSelector::onTimer, this)); + this, get_clock(), period_ns, std::bind(&ExternalCmdSelector::onTimer, this), + callback_group_subscribers_); } void ExternalCmdSelector::onLocalControlCmd(const ExternalControlCommand::ConstSharedPtr msg) 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 3fd02bfa8a68..cbfb469a72fa 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 @@ -462,5 +462,6 @@ AutowareStateMonitorNode::AutowareStateMonitorNode() // Timer const auto period_ns = rclcpp::Rate(update_rate_).period(); timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&AutowareStateMonitorNode::onTimer, this)); + this, get_clock(), period_ns, std::bind(&AutowareStateMonitorNode::onTimer, this), + callback_group_subscribers_); } From 6e40c8b125ee88553260ea93e7a0d53fec734da5 Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Mon, 14 Mar 2022 18:09:39 +0900 Subject: [PATCH 05/15] fix(fault_injection): fix launch_testing (#489) * fix(fault_injection): fix launch_testing Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * add label Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * add todo comment Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> --- simulator/fault_injection/CMakeLists.txt | 26 +++++++++----- .../test/test_fault_injection_node.test.py | 35 +++++++------------ 2 files changed, 30 insertions(+), 31 deletions(-) diff --git a/simulator/fault_injection/CMakeLists.txt b/simulator/fault_injection/CMakeLists.txt index 3af2fc1e4ed1..8a091f613f0b 100644 --- a/simulator/fault_injection/CMakeLists.txt +++ b/simulator/fault_injection/CMakeLists.txt @@ -42,16 +42,24 @@ if(BUILD_TESTING) fault_injection_node_component ) - # TODO: Comment out until the error in CI is resolved. # launch_testing - # find_package(launch_testing_ament_cmake REQUIRED) - # add_launch_test(test/test_fault_injection_node.test.py) - - # install(DIRECTORY - # test/config - # test/launch - # DESTINATION share/${PROJECT_NAME}/ - # ) + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test( + test/test_fault_injection_node.test.py + TARGET test_fault_injection_launch_test + ) + # TODO(KeisukeShima): Remove in Humble + set_tests_properties( + test_fault_injection_launch_test + PROPERTIES + LABELS "launch_test" + ) + + install(DIRECTORY + test/config + test/launch + DESTINATION share/${PROJECT_NAME}/ + ) endif() ## Package diff --git a/simulator/fault_injection/test/test_fault_injection_node.test.py b/simulator/fault_injection/test/test_fault_injection_node.test.py index 6f77ad00f15b..887c39f449a2 100644 --- a/simulator/fault_injection/test/test_fault_injection_node.test.py +++ b/simulator/fault_injection/test/test_fault_injection_node.test.py @@ -67,7 +67,7 @@ def setUp(self): # Create a ROS node for tests self.test_node = rclpy.create_node("test_node") self.event_name = "cpu_temperature" - self.evaluation_time = 10.0 + self.evaluation_time = 0.5 # 500ms def tearDown(self): self.test_node.destroy_node() @@ -105,36 +105,27 @@ def test_node_link(self): DiagnosticArray, "/diagnostics", lambda msg: msg_buffer.append(msg), 10 ) - # Wait until the talker transmits messages over the ROS topic - item = FaultInjectionEvent(name=self.event_name, level=FaultInjectionEvent.ERROR) - msg = SimulationEvents(fault_injection_events=[item]) - pub_events.publish(msg) + # Test init state. + # Expect fault_injection_node does not publish /diagnostics.status + # while the talker does not to publish + # Wait until the talker transmits two messages over the ROS topic end_time = time.time() + self.evaluation_time while time.time() < end_time: rclpy.spin_once(self.test_node, timeout_sec=0.1) - self.assertGreaterEqual(self.get_num_valid_data(msg_buffer, DiagnosticStatus.ERROR), 1) - - def test_node_init_state(self): - """ - Test init state. - - Expect fault_injection_node does not publish /diagnostics.status - while the talker does not to publish - """ - msg_buffer = [] - - self.test_node.create_subscription( - DiagnosticArray, "/diagnostics", lambda msg: msg_buffer.append(msg), 10 - ) + # Return False if no valid data is received + self.assertEqual(self.get_num_valid_data(msg_buffer, DiagnosticStatus.ERROR), 0) - # Wait until the talker transmits two messages over the ROS topic + # Test node linkage. + # Wait until the talker transmits messages over the ROS topic + item = FaultInjectionEvent(name=self.event_name, level=FaultInjectionEvent.ERROR) + msg = SimulationEvents(fault_injection_events=[item]) + pub_events.publish(msg) end_time = time.time() + self.evaluation_time while time.time() < end_time: rclpy.spin_once(self.test_node, timeout_sec=0.1) - # Return False if no valid data is received - self.assertEqual(self.get_num_valid_data(msg_buffer, DiagnosticStatus.ERROR), 0) + self.assertGreaterEqual(self.get_num_valid_data(msg_buffer, DiagnosticStatus.ERROR), 1) @launch_testing.post_shutdown_test() From 4f4b727bc83cbd3dccd93c3b9ef64556ce93e400 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 14 Mar 2022 12:18:43 +0000 Subject: [PATCH 06/15] chore: sync files (#518) Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .github/workflows/deploy-docs.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index bb7addaf5390..27b80781403f 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -38,4 +38,4 @@ jobs: uses: autowarefoundation/autoware-github-actions/deploy-docs@v1 with: token: ${{ secrets.GITHUB_TOKEN }} - latest: ${{ github.ref_name == github.event.repository.default_branch }} + latest: ${{ github.event_name != 'pull_request_target' && github.ref_name == github.event.repository.default_branch }} From fa2bca879d8f8a93398571e0ea8078bdb85cf83c Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 15 Mar 2022 11:08:40 +0900 Subject: [PATCH 07/15] fix(behavior_velocity): fix detection area ,stop line,traffic lights stop line insertion ,occlusion spot (#505) Signed-off-by: tanaka3 --- .../include/utilization/util.hpp | 7 ++++ .../src/scene_module/blind_spot/scene.cpp | 12 +++++- .../src/scene_module/crosswalk/util.cpp | 22 +++-------- .../src/scene_module/detection_area/scene.cpp | 12 ++---- .../scene_no_stopping_area.cpp | 11 ++---- .../risk_predictive_braking.cpp | 21 ++++------ .../src/scene_module/stop_line/scene.cpp | 13 ++----- .../src/scene_module/traffic_light/scene.cpp | 18 +++------ .../virtual_traffic_light/scene.cpp | 12 ++---- .../src/utilization/util.cpp | 38 +++++++++++++++++++ 10 files changed, 87 insertions(+), 79 deletions(-) diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 1fe075dc18c9..cd7b029cf165 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -70,6 +71,8 @@ struct PointWithSearchRangeIndex using autoware_auto_planning_msgs::msg::PathWithLaneId; using Point2d = boost::geometry::model::d2::point_xy; using Polygons2d = std::vector; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; namespace planning_utils { inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; } @@ -108,6 +111,10 @@ inline geometry_msgs::msg::Pose getPose( return traj.points.at(idx).pose; } void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, Polygons2d & polys); +void setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input); +void insertVelocity( + PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, + size_t & insert_index, const double min_distance = 0.001); inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); } inline double square(const double & a) { return a * a; } diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index b0e8d4c8b74b..1a4bff3b0dc0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -242,11 +242,13 @@ bool BlindSpotModule::generateStopLine( } /* insert judge point */ - const int pass_judge_idx_ip = std::min( + // need to remove const because pass judge idx will be changed by insert stop point + int pass_judge_idx_ip = std::min( static_cast(path_ip.points.size()) - 1, std::max(stop_idx_ip - pass_judge_idx_dist, 0)); if (has_prior_stopline || stop_idx_ip == pass_judge_idx_ip) { *pass_judge_line_idx = *stop_line_idx; } else { + //! insertPoint check if there is no duplicated point *pass_judge_line_idx = insertPoint(pass_judge_idx_ip, path_ip, path); ++(*stop_line_idx); // stop index is incremented by judge line insertion } @@ -309,6 +311,14 @@ int BlindSpotModule::insertPoint( // copy from previous point inserted_point = inout_path->points.at(std::max(insert_idx - 1, 0)); inserted_point.point.pose = path_ip.points[insert_idx_ip].point.pose; + constexpr double min_dist = 0.001; + //! avoid to insert duplicated point + if ( + planning_utils::calcDist2d(inserted_point, inout_path->points.at(insert_idx).point) < + min_dist) { + inout_path->points.at(insert_idx).point.longitudinal_velocity_mps = 0.0; + return insert_idx; + } inout_path->points.insert(it, inserted_point); } return insert_idx; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp index 1f6b8d9a9eb4..9aea92e18a35 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp @@ -157,15 +157,9 @@ bool insertTargetVelocityPoint( } // ---------------- - // insert target point - output.points.insert( - output.points.begin() + insert_target_point_idx, target_point_with_lane_id); + planning_utils::insertVelocity( + output, target_point_with_lane_id, velocity, insert_target_point_idx); - // insert 0 velocity after target point - for (size_t j = insert_target_point_idx; j < output.points.size(); ++j) { - output.points.at(j).point.longitudinal_velocity_mps = - std::min(static_cast(velocity), output.points.at(j).point.longitudinal_velocity_mps); - } return true; } return false; @@ -300,15 +294,9 @@ bool insertTargetVelocityPoint( } // ---------------- - // insert target point - output.points.insert( - output.points.begin() + insert_target_point_idx, target_point_with_lane_id); - - // insert 0 velocity after target point - for (size_t j = insert_target_point_idx; j < output.points.size(); ++j) { - output.points.at(j).point.longitudinal_velocity_mps = - std::min(static_cast(velocity), output.points.at(j).point.longitudinal_velocity_mps); - } + // insert target point or replace with 0 velocity if same points found + planning_utils::insertVelocity( + output, target_point_with_lane_id, velocity, insert_target_point_idx); return true; } return false; diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 1d8c136f0363..b3d89c333144 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -375,7 +375,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId DetectionAreaModule::insertStop { auto output_path = path; - const auto insert_idx = stop_point.first + 1; + size_t insert_idx = static_cast(stop_point.first + 1); const auto stop_pose = stop_point.second; // To PathPointWithLaneId @@ -384,14 +384,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId DetectionAreaModule::insertStop stop_point_with_lane_id.point.pose = stop_pose; stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; - // Insert stop point - output_path.points.insert(output_path.points.begin() + insert_idx, stop_point_with_lane_id); - - // Insert 0 velocity after stop point - for (size_t j = insert_idx; j < output_path.points.size(); ++j) { - output_path.points.at(j).point.longitudinal_velocity_mps = 0.0; - } - + // Insert stop point or replace with zero velocity + planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx); return output_path; } diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index 00eb4b319fe9..deea9e26c160 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -397,7 +397,7 @@ bool NoStoppingAreaModule::isStoppable( void NoStoppingAreaModule::insertStopPoint( autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) { - const auto insert_idx = stop_point.first + 1; + size_t insert_idx = static_cast(stop_point.first + 1); const auto stop_pose = stop_point.second; // To PathPointWithLaneId @@ -406,13 +406,8 @@ void NoStoppingAreaModule::insertStopPoint( stop_point_with_lane_id.point.pose = stop_pose; stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; - // Insert stop point - path.points.insert(path.points.begin() + insert_idx, stop_point_with_lane_id); - - // Insert 0 velocity after stop point - for (size_t j = insert_idx; j < path.points.size(); ++j) { - path.points.at(j).point.longitudinal_velocity_mps = 0.0; - } + // Insert stop point or replace with zero velocity + planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); } boost::optional NoStoppingAreaModule::createTargetPoint( diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index cb7e56245a4c..94150d0a43fc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -85,23 +85,16 @@ int insertSafeVelocityToPath( } PathPointWithLaneId inserted_point; inserted_point = inout_path->points.at(closest_idx); - int insert_idx = closest_idx; + size_t insert_idx = closest_idx; // insert velocity to path if distance is not too close else insert new collision point // if original path has narrow points it's better to set higher distance threshold - if (planning_utils::calcDist2d(in_pose, inserted_point.point) > 0.3) { - if (isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { - ++insert_idx; - } - // return if index is after the last path point - if (insert_idx == static_cast(inout_path->points.size())) { - return -1; - } - auto it = inout_path->points.begin() + insert_idx; - inserted_point = inout_path->points.at(closest_idx); - inserted_point.point.pose = in_pose; - inout_path->points.insert(it, inserted_point); + if (isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { + ++insert_idx; + if (insert_idx == static_cast(inout_path->points.size())) return -1; } - setVelocityFrom(insert_idx, safe_vel, inout_path); + // return if index is after the last path point + inserted_point.point.pose = in_pose; + planning_utils::insertVelocity(*inout_path, inserted_point, safe_vel, insert_idx); return 0; } diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp index de3f52488e78..bc8b344a784c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp @@ -166,22 +166,17 @@ autoware_auto_planning_msgs::msg::PathWithLaneId StopLineModule::insertStopPose( auto modified_path = path; // Insert stop pose to between segment start and end - const int insert_index = stop_pose_with_index.index + 1; + size_t insert_index = static_cast(stop_pose_with_index.index + 1); auto stop_point_with_lane_id = modified_path.points.at(insert_index); stop_point_with_lane_id.point.pose = stop_pose_with_index.pose; stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; // Update first stop index - first_stop_path_point_index_ = insert_index; + first_stop_path_point_index_ = static_cast(insert_index); debug_data_.stop_pose = stop_point_with_lane_id.point.pose; - // Insert stop point - modified_path.points.insert(modified_path.points.begin() + insert_index, stop_point_with_lane_id); - - // Insert 0 velocity after stop point - for (size_t j = insert_index; j < modified_path.points.size(); ++j) { - modified_path.points.at(j).point.longitudinal_velocity_mps = 0.0; - } + // Insert stop point or replace with zero velocity + planning_utils::insertVelocity(modified_path, stop_point_with_lane_id, 0.0, insert_index); // Get stop point and stop factor { diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index ce455ba27859..7b68b4c3a2a1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -460,23 +460,17 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP modified_path = input; // Create stop pose - const int target_velocity_point_idx = std::max(static_cast(insert_target_point_idx - 1), 0); + size_t target_velocity_point_idx = std::max(static_cast(insert_target_point_idx - 1), 0); auto target_point_with_lane_id = modified_path.points.at(target_velocity_point_idx); target_point_with_lane_id.point.pose.position.x = target_point.x(); target_point_with_lane_id.point.pose.position.y = target_point.y(); target_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; - // Insert stop pose into path - modified_path.points.insert( - modified_path.points.begin() + insert_target_point_idx, target_point_with_lane_id); - debug_data_.stop_poses.push_back(target_point_with_lane_id.point.pose); - - // insert 0 velocity after target point - for (size_t j = insert_target_point_idx; j < modified_path.points.size(); ++j) { - modified_path.points.at(j).point.longitudinal_velocity_mps = 0.0; - } - if (target_velocity_point_idx < first_stop_path_point_index_) { - first_stop_path_point_index_ = target_velocity_point_idx; + // Insert stop pose into path or replace with zero velocity + planning_utils::insertVelocity( + modified_path, target_point_with_lane_id, 0.0, target_velocity_point_idx); + if (static_cast(target_velocity_point_idx) < first_stop_path_point_index_) { + first_stop_path_point_index_ = static_cast(target_velocity_point_idx); debug_data_.first_stop_pose = target_point_with_lane_id.point.pose; } diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index 5c0723836da9..76fec2743200 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -279,17 +279,11 @@ size_t insertStopVelocityAtCollision( return 0; } - const auto insert_index = offset_segment.index + 1; + auto insert_index = static_cast(offset_segment.index + 1); auto insert_point = path->points.at(insert_index); insert_point.point.pose = interpolated_pose; - - path->points.insert(path->points.begin() + insert_index, insert_point); - - // Insert 0 velocity after stop point - for (size_t i = insert_index; i < path->points.size(); ++i) { - path->points.at(i).point.longitudinal_velocity_mps = 0.0; - } - + // Insert 0 velocity after stop point or replace velocity with 0 + behavior_velocity_planner::planning_utils::insertVelocity(*path, insert_point, 0.0, insert_index); return insert_index; } } // namespace diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 937818d17e78..57906fe36c01 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -59,6 +59,44 @@ SearchRangeIndex getPathIndexRangeIncludeLaneId( } return search_range; } + +void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input) +{ + for (size_t i = begin_idx; i < input->points.size(); ++i) { + input->points.at(i).point.longitudinal_velocity_mps = + std::min(static_cast(vel), input->points.at(i).point.longitudinal_velocity_mps); + } + return; +} + +void insertVelocity( + PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, + size_t & insert_index, const double min_distance) +{ + bool already_has_path_point = false; + // consider front/back point is near to insert point or not + int min_idx = std::max(0, static_cast(insert_index - 1)); + int max_idx = + std::min(static_cast(insert_index + 1), static_cast(path.points.size() - 1)); + for (int i = min_idx; i <= max_idx; i++) { + if ( + tier4_autoware_utils::calcDistance2d(path.points.at(static_cast(i)), path_point) < + min_distance) { + path.points.at(i).point.longitudinal_velocity_mps = 0; + already_has_path_point = true; + insert_index = static_cast(i); + // set velocity from is going to insert min velocity later + break; + } + } + //! insert velocity point only if there is no close point on path + if (!already_has_path_point) { + path.points.insert(path.points.begin() + insert_index, path_point); + } + // set zero velocity from insert index + setVelocityFromIndex(insert_index, v, &path); +} + Polygon2d toFootprintPolygon(const autoware_auto_perception_msgs::msg::PredictedObject & object) { Polygon2d obj_footprint; From f9166504344e728eac05c78750b80acc80d101d2 Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Tue, 15 Mar 2022 17:11:40 +0900 Subject: [PATCH 08/15] fix(lane_change, pull_over): fix predicted path size is small case (#517) * fix(lane_change):fix predicted path size is small case * fix(pull_over):fix predicted path size is small case --- .../behavior_path_planner/src/scene_module/lane_change/util.cpp | 2 ++ .../behavior_path_planner/src/scene_module/pull_over/util.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 650b23c5f40d..8038c77e3b5f 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -390,6 +390,7 @@ bool isLaneChangePathSafe( const auto & obj = dynamic_objects->objects.at(i); std::vector predicted_paths; if (ros_parameters.use_all_predicted_path) { + predicted_paths.resize(obj.kinematics.predicted_paths.size()); std::copy( obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), predicted_paths.begin()); @@ -425,6 +426,7 @@ bool isLaneChangePathSafe( const auto & obj = dynamic_objects->objects.at(i); std::vector predicted_paths; if (ros_parameters.use_all_predicted_path) { + predicted_paths.resize(obj.kinematics.predicted_paths.size()); std::copy( obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), predicted_paths.begin()); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index fa4f55a5c026..303565464b2e 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -424,6 +424,7 @@ bool isPullOverPathSafe( const auto & obj = dynamic_objects->objects.at(i); std::vector predicted_paths; if (ros_parameters.use_all_predicted_path) { + predicted_paths.resize(obj.kinematics.predicted_paths.size()); std::copy( obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), predicted_paths.begin()); @@ -459,6 +460,7 @@ bool isPullOverPathSafe( const auto & obj = dynamic_objects->objects.at(i); std::vector predicted_paths; if (ros_parameters.use_all_predicted_path) { + predicted_paths.resize(obj.kinematics.predicted_paths.size()); std::copy( obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), predicted_paths.begin()); From ebde6f946df33274d91e15cee369896cbe050ff6 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Tue, 15 Mar 2022 17:14:18 +0900 Subject: [PATCH 09/15] chore: save engine file in the same directory as onnx file (#519) Signed-off-by: wep21 --- .../traffic_light_classifier/CMakeLists.txt | 4 +-- .../src/cnn_classifier.cpp | 4 +-- .../utils/trt_common.cpp | 25 ++++++++++++------- .../utils/trt_common.hpp | 4 +-- .../CMakeLists.txt | 5 ++-- .../src/nodelet.cpp | 23 ++++++++++------- 6 files changed, 36 insertions(+), 29 deletions(-) diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt index 2f07b8d328ce..b89ee6f4ca3b 100644 --- a/perception/traffic_light_classifier/CMakeLists.txt +++ b/perception/traffic_light_classifier/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(traffic_light_classifier) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Werror) @@ -133,7 +133,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} ${CUDNN_LIBRARY} - ${Boost_LIBRARIES} + stdc++fs ) ament_auto_add_library(traffic_light_classifier_nodelet SHARED diff --git a/perception/traffic_light_classifier/src/cnn_classifier.cpp b/perception/traffic_light_classifier/src/cnn_classifier.cpp index 40f1757e6329..9147fbb8773a 100644 --- a/perception/traffic_light_classifier/src/cnn_classifier.cpp +++ b/perception/traffic_light_classifier/src/cnn_classifier.cpp @@ -42,9 +42,7 @@ CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) readLabelfile(label_file_path, labels_); - std::string cache_dir = - ament_index_cpp::get_package_share_directory("traffic_light_classifier") + "/data"; - trt_ = std::make_shared(model_file_path, cache_dir, precision); + trt_ = std::make_shared(model_file_path, precision); trt_->setup(); } diff --git a/perception/traffic_light_classifier/utils/trt_common.cpp b/perception/traffic_light_classifier/utils/trt_common.cpp index 6e10d7d71ab5..a920d47fa295 100644 --- a/perception/traffic_light_classifier/utils/trt_common.cpp +++ b/perception/traffic_light_classifier/utils/trt_common.cpp @@ -14,6 +14,14 @@ #include +#if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) +#include +namespace fs = ::std::filesystem; +#else +#include +namespace fs = ::std::experimental::filesystem; +#endif + #include #include @@ -29,9 +37,8 @@ void check_error(const ::cudaError_t e, decltype(__FILE__) f, decltype(__LINE__) } } -TrtCommon::TrtCommon(std::string model_path, std::string cache_dir, std::string precision) +TrtCommon::TrtCommon(std::string model_path, std::string precision) : model_file_path_(model_path), - cache_dir_(cache_dir), precision_(precision), input_name_("input_0"), output_name_("output_0"), @@ -42,20 +49,20 @@ TrtCommon::TrtCommon(std::string model_path, std::string cache_dir, std::string void TrtCommon::setup() { - const boost::filesystem::path path(model_file_path_); + const fs::path path(model_file_path_); std::string extension = path.extension().string(); - if (boost::filesystem::exists(path)) { + if (fs::exists(path)) { if (extension == ".engine") { loadEngine(model_file_path_); } else if (extension == ".onnx") { - std::string cache_engine_path = cache_dir_ + "/" + path.stem().string() + ".engine"; - const boost::filesystem::path cache_path(cache_engine_path); - if (boost::filesystem::exists(cache_path)) { - loadEngine(cache_engine_path); + fs::path cache_engine_path{model_file_path_}; + cache_engine_path.replace_extension("engine"); + if (fs::exists(cache_engine_path)) { + loadEngine(cache_engine_path.string()); } else { logger_.log(nvinfer1::ILogger::Severity::kINFO, "start build engine"); - buildEngineFromOnnx(model_file_path_, cache_engine_path); + buildEngineFromOnnx(model_file_path_, cache_engine_path.string()); logger_.log(nvinfer1::ILogger::Severity::kINFO, "end build engine"); } } else { diff --git a/perception/traffic_light_classifier/utils/trt_common.hpp b/perception/traffic_light_classifier/utils/trt_common.hpp index fb3f495fcdcf..7c172b671967 100644 --- a/perception/traffic_light_classifier/utils/trt_common.hpp +++ b/perception/traffic_light_classifier/utils/trt_common.hpp @@ -18,8 +18,6 @@ #include #include -#include - #include <./cudnn.h> #include #include @@ -110,7 +108,7 @@ Tn::UniquePtr make_unique() class TrtCommon { public: - TrtCommon(std::string model_path, std::string cache_dir, std::string precision); + TrtCommon(std::string model_path, std::string precision); ~TrtCommon() {} bool loadEngine(std::string engine_file_path); diff --git a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt index b20d06f91f5a..2dd2f6979c43 100644 --- a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) endif() @@ -123,8 +123,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${CUDA_INCLUDE_DIRS} ) - set(CMAKE_CXX_FLAGS "-O2 -Wall -Wunused-variable ${CMAKE_CXX_FLAGS} -fpic -std=c++11 -pthread") - ### ssd ### ament_auto_add_library(ssd SHARED lib/src/trt_ssd.cpp @@ -149,6 +147,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) target_link_libraries(traffic_light_ssd_fine_detector_nodelet ${OpenCV_LIB} + stdc++fs ssd ) diff --git a/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp b/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp index c6fe21016567..5e5c2126fb5c 100644 --- a/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp @@ -14,9 +14,16 @@ #include "traffic_light_ssd_fine_detector/nodelet.hpp" -#include #include +#if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) +#include +namespace fs = ::std::filesystem; +#else +#include +namespace fs = ::std::experimental::filesystem; +#endif + #include #include #include @@ -36,25 +43,23 @@ TrafficLightSSDFineDetectorNodelet::TrafficLightSSDFineDetectorNodelet( using std::placeholders::_1; using std::placeholders::_2; - std::string package_path = - ament_index_cpp::get_package_share_directory("traffic_light_ssd_fine_detector"); - std::string data_path = package_path + "/data/"; - std::string engine_path = package_path + "/data/mb2-ssd-lite.engine"; - std::ifstream fs(engine_path); std::vector labels; const int max_batch_size = this->declare_parameter("max_batch_size", 8); const std::string onnx_file = this->declare_parameter("onnx_file"); const std::string label_file = this->declare_parameter("label_file"); const std::string mode = this->declare_parameter("mode", "FP32"); + fs::path engine_path{onnx_file}; + engine_path.replace_extension("engine"); + if (readLabelFile(label_file, labels)) { if (!getTlrIdFromLabel(labels, tlr_id_)) { RCLCPP_ERROR(this->get_logger(), "Could not find tlr id"); } } - if (fs.is_open()) { - RCLCPP_INFO(this->get_logger(), "Found %s", engine_path.c_str()); + if (fs::exists(engine_path)) { + RCLCPP_INFO(this->get_logger(), "Found %s", engine_path.string().c_str()); net_ptr_.reset(new ssd::Net(engine_path, false)); if (max_batch_size != net_ptr_->getMaxBatchSize()) { RCLCPP_INFO( @@ -69,7 +74,7 @@ TrafficLightSSDFineDetectorNodelet::TrafficLightSSDFineDetectorNodelet( } else { RCLCPP_INFO( this->get_logger(), "Could not find %s, try making TensorRT engine from onnx", - engine_path.c_str()); + engine_path.string().c_str()); net_ptr_.reset(new ssd::Net(onnx_file, mode, max_batch_size)); net_ptr_->save(engine_path); } From 016fd47d51a37e7816fdcab2a49d37af00fa2d6e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 15 Mar 2022 18:38:10 +0900 Subject: [PATCH 10/15] fix(side_shift): side shift unable to properly reset (#442) * fix(side_shift):side shift unable to properly reset Add comparison between lateral offset and shift point. the last shift point was compared. Signed-off-by: Muhammad Zulfaqar Azmi * ci(pre-commit): autofix Signed-off-by: Muhammad Zulfaqar Azmi * fix: extend drivable areas relative to ego position Signed-off-by: Muhammad Zulfaqar Azmi * fix: constraint request time to prevent input abuse In this fix, request time is constraint according to time_to_start_shifting in the side_shift.param.yaml. Signed-off-by: Muhammad Zulfaqar Azmi * fix: refactor condition Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp Co-authored-by: Yukihiro Saito Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp Co-authored-by: Yukihiro Saito Signed-off-by: Muhammad Zulfaqar Azmi * chore: variable name refactoring Signed-off-by: Muhammad Zulfaqar Azmi * fix: added new parameter so that the request time is configurable The fix also added stl remove_if algorithm to remove shift points that exceed maximum threshold Signed-off-by: Muhammad Zulfaqar Azmi * refactor: replace chrono to rclcpp::Time Signed-off-by: Muhammad Zulfaqar Azmi * Revert "refactor: replace chrono to rclcpp::Time" This reverts commit 76823c6120e030eda068a65d88b2acd2d0ec37fa. * refactor: replace chrono to rclcpp::Time Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yukihiro Saito --- .../config/side_shift/side_shift.param.yaml | 1 + .../side_shift/side_shift_module.hpp | 9 +++ .../src/behavior_path_planner_node.cpp | 1 + .../side_shift/side_shift_module.cpp | 75 ++++++++++++++----- 4 files changed, 69 insertions(+), 17 deletions(-) diff --git a/planning/behavior_path_planner/config/side_shift/side_shift.param.yaml b/planning/behavior_path_planner/config/side_shift/side_shift.param.yaml index 79044041b488..815301d55aa5 100644 --- a/planning/behavior_path_planner/config/side_shift/side_shift.param.yaml +++ b/planning/behavior_path_planner/config/side_shift/side_shift.param.yaml @@ -6,3 +6,4 @@ shifting_lateral_jerk: 0.2 min_shifting_distance: 5.0 min_shifting_speed: 5.56 + shift_request_time_limit: 1.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index 7ac7b6ca2fbd..c51f2661c812 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -32,6 +33,9 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; +using std::chrono::duration; +using std::chrono::high_resolution_clock; +using std::chrono::time_point; using tier4_planning_msgs::msg::LateralOffset; struct SideShiftParameters @@ -44,6 +48,7 @@ struct SideShiftParameters double drivable_area_resolution; double drivable_area_width; double drivable_area_height; + double shift_request_time_limit; }; class SideShiftModule : public SceneModuleInterface @@ -54,6 +59,8 @@ class SideShiftModule : public SceneModuleInterface bool isExecutionRequested() const override; bool isExecutionReady() const override; + bool isReadyForNextRequest( + const double & min_request_time_sec, bool override_requests = false) const noexcept; BT::NodeStatus updateState() override; void updateData() override; BehaviorModuleOutput plan() override; @@ -107,6 +114,8 @@ class SideShiftModule : public SceneModuleInterface inline PoseStamped getEgoPose() const { return *(planner_data_->self_pose); } PathWithLaneId calcCenterLinePath( const std::shared_ptr & planner_data, const PoseStamped & pose) const; + + mutable rclcpp::Time last_requested_shift_change_time_{clock_->now()}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 7ea836f2f4f9..0517fd2bef00 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -190,6 +190,7 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() p.shifting_lateral_jerk = dp("shifting_lateral_jerk", 0.2); p.min_shifting_distance = dp("min_shifting_distance", 5.0); p.min_shifting_speed = dp("min_shifting_speed", 5.56); + p.shift_request_time_limit = dp("shift_request_time_limit", 1.0); return p; } diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 2f601f600cfe..158eabd37661 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -115,11 +115,40 @@ bool SideShiftModule::isExecutionReady() const return true; // TODO(Horibe) is it ok to say "always safe"? } +bool SideShiftModule::isReadyForNextRequest( + const double & min_request_time_sec, bool override_requests) const noexcept +{ + rclcpp::Time current_time = clock_->now(); + const auto interval_from_last_request_sec = current_time - last_requested_shift_change_time_; + + if (interval_from_last_request_sec.seconds() >= min_request_time_sec && !override_requests) { + last_requested_shift_change_time_ = current_time; + return true; + } + + return false; +} + BT::NodeStatus SideShiftModule::updateState() { // Never return the FAILURE. When the desired offset is zero and the vehicle is in the original // drivable area,this module can stop the computation and return SUCCESS. + const auto isOffsetDiffAlmostZero = [this]() noexcept { + const auto last_sp = path_shifter_.getLastShiftPoint(); + if (last_sp) { + const auto length = std::fabs(last_sp.get().length); + const auto lateral_offset = std::fabs(lateral_offset_); + const auto offset_diff = lateral_offset - length; + if (!isAlmostZero(offset_diff)) { + lateral_offset_change_request_ = true; + return false; + } + } + return true; + }(); + + const bool no_offset_diff = isOffsetDiffAlmostZero; const bool no_request = isAlmostZero(lateral_offset_); const auto no_shifted_plan = [&]() { @@ -137,7 +166,7 @@ BT::NodeStatus SideShiftModule::updateState() getLogger(), "ESS::updateState() : no_request = %d, no_shifted_plan = %d", no_request, no_shifted_plan); - if (no_request && no_shifted_plan) { + if (no_request && no_shifted_plan && no_offset_diff) { current_state_ = BT::NodeStatus::SUCCESS; } else { current_state_ = BT::NodeStatus::RUNNING; @@ -182,14 +211,18 @@ bool SideShiftModule::addShiftPoint() }; // remove shift points on a far position. - for (int i = static_cast(shift_points.size()) - 1; i >= 0; --i) { - const auto dist_to_start = calcLongitudinal(shift_points.at(i)); - const double remove_threshold = - std::max(planner_data_->self_odometry->twist.twist.linear.x * 1.0 /* sec */, 2.0 /* m */); - if (dist_to_start > remove_threshold) { // TODO(Horibe) - shift_points.erase(shift_points.begin() + i); - } - } + const auto remove_iter = std::remove_if( + shift_points.begin(), shift_points.end(), [this, calcLongitudinal](const ShiftPoint & sp) { + const auto dist_to_start = calcLongitudinal(sp); + constexpr double max_remove_threshold_time = 1.0; // [s] + constexpr double max_remove_threshold_dist = 2.0; // [m] + const auto ego_current_speed = planner_data_->self_odometry->twist.twist.linear.x; + const auto remove_threshold = + std::max(ego_current_speed * max_remove_threshold_time, max_remove_threshold_dist); + return (dist_to_start > remove_threshold); + }); + + shift_points.erase(remove_iter, shift_points.end()); // check if the new_shift_point has conflicts with existing shift points. const auto new_sp = calcShiftPoint(); @@ -289,10 +322,16 @@ void SideShiftModule::onLateralOffset(const LateralOffset::ConstSharedPtr latera return; } + if (parameters_.shift_request_time_limit < parameters_.time_to_start_shifting) { + RCLCPP_DEBUG( + getLogger(), "Shift request time might be too low. Generated trajectory might be wavy"); + } // new offset is requested. - lateral_offset_change_request_ = true; + if (isReadyForNextRequest(parameters_.shift_request_time_limit)) { + lateral_offset_change_request_ = true; - lateral_offset_ = new_lateral_offset; + lateral_offset_ = new_lateral_offset; + } } ShiftPoint SideShiftModule::calcShiftPoint() const @@ -307,7 +346,7 @@ ShiftPoint SideShiftModule::calcShiftPoint() const const double dist_to_end = [&]() { const double shift_length = lateral_offset_ - getClosestShiftLength(); const double jerk_shifting_distance = path_shifter_.calcLongitudinalDistFromJerk( - shift_length, p.shifting_lateral_jerk, std::min(ego_speed, p.min_shifting_speed)); + shift_length, p.shifting_lateral_jerk, std::max(ego_speed, p.min_shifting_speed)); const double shifting_distance = std::max(jerk_shifting_distance, p.min_shifting_distance); const double dist_to_end = dist_to_start + shifting_distance; RCLCPP_DEBUG( @@ -341,8 +380,8 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const { const auto itr = std::minmax_element(path->shift_length.begin(), path->shift_length.end()); - const double threshold = 0.1; - const double margin = 0.5; + constexpr double threshold = 0.1; + constexpr double margin = 0.5; const double right_offset = std::min(*itr.first - (*itr.first < -threshold ? margin : 0.0), 0.0); const double left_offset = std::max(*itr.second + (*itr.first > threshold ? margin : 0.0), 0.0); @@ -360,16 +399,18 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const PoseStamped SideShiftModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const { const auto ego_pose = getEgoPose(); + if (prev_path.path.points.empty()) { + return ego_pose; + } // un-shifted fot current ideal pose const auto closest = tier4_autoware_utils::findNearestIndex(prev_path.path.points, ego_pose.pose.position); - PoseStamped unshifted_pose{}; - unshifted_pose.header = ego_pose.header; - unshifted_pose.pose = prev_path.path.points.at(closest).point.pose; + PoseStamped unshifted_pose = ego_pose; util::shiftPose(&unshifted_pose.pose, -prev_path.shift_length.at(closest)); + unshifted_pose.pose.orientation = ego_pose.pose.orientation; return unshifted_pose; } From 672f6d7ed1d8439551c03d8d7721e261e77d2e4b Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 15 Mar 2022 20:18:18 +0900 Subject: [PATCH 11/15] feat: add cpu and net diag in resource monitoring (#521) --- .../diagnostic_aggregator/system.param.yaml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index 313c88401658..93d3813a6755 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -76,6 +76,18 @@ contains: [": CPU Thermal Throttling"] timeout: 3.0 + frequency: + type: diagnostic_aggregator/GenericAnalyzer + path: frequency + contains: [": CPU Frequency"] + timeout: 3.0 + + load_average: + type: diagnostic_aggregator/GenericAnalyzer + path: load_average + contains: [": CPU Load Average"] + timeout: 3.0 + gpu: type: diagnostic_aggregator/AnalyzerGroup path: gpu @@ -124,6 +136,12 @@ contains: [": Network Usage"] timeout: 3.0 + network_traffic: + type: diagnostic_aggregator/GenericAnalyzer + path: network_traffic + contains: [": Network Traffic"] + timeout: 3.0 + storage: type: diagnostic_aggregator/AnalyzerGroup path: storage From 976712bcc499b5f734c95a0ee321f9b2fc0c7c8b Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 15 Mar 2022 20:56:10 +0900 Subject: [PATCH 12/15] feat: add process diag in resource monitoring (#522) --- .../diagnostic_aggregator/system.param.yaml | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index 93d3813a6755..2cea6cc97cb3 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -157,3 +157,25 @@ path: usage contains: [": HDD Usage"] timeout: 3.0 + + process: + type: diagnostic_aggregator/AnalyzerGroup + path: process + analyzers: + high_load: + type: diagnostic_aggregator/GenericAnalyzer + path: high_load + contains: [": High-load"] + timeout: 3.0 + + high_mem: + type: diagnostic_aggregator/GenericAnalyzer + path: high_mem + contains: [": High-mem"] + timeout: 3.0 + + tasks_summary: + type: diagnostic_aggregator/GenericAnalyzer + path: tasks_summary + contains: [": Tasks Summary"] + timeout: 3.0 From f9c2be78d4a5e643a2573a7fc9fff5aab099c947 Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Tue, 15 Mar 2022 22:26:27 +0900 Subject: [PATCH 13/15] chore(dummy_diag_publisher): add exec_depend (#523) * chore(dummy_diag_publisher): add exec_depend Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * Update system/dummy_diag_publisher/package.xml Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- system/dummy_diag_publisher/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/system/dummy_diag_publisher/package.xml b/system/dummy_diag_publisher/package.xml index 15574771ec2f..d6cd9847a84a 100644 --- a/system/dummy_diag_publisher/package.xml +++ b/system/dummy_diag_publisher/package.xml @@ -14,6 +14,8 @@ tier4_autoware_utils rqt_reconfigure + rqt_robot_monitor + rqt_runtime_monitor ament_lint_auto autoware_lint_common From 7c067b8bda6495313f9c074e78e4e3cec40e93e4 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Wed, 16 Mar 2022 10:56:11 +0900 Subject: [PATCH 14/15] fix(behavior_velocity): add traffic light stop point insert (#526) Signed-off-by: tanaka3 --- .../src/scene_module/traffic_light/scene.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index 7b68b4c3a2a1..e509b80b3464 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -460,15 +460,15 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP modified_path = input; // Create stop pose - size_t target_velocity_point_idx = std::max(static_cast(insert_target_point_idx - 1), 0); + const int target_velocity_point_idx = std::max(static_cast(insert_target_point_idx - 1), 0); auto target_point_with_lane_id = modified_path.points.at(target_velocity_point_idx); target_point_with_lane_id.point.pose.position.x = target_point.x(); target_point_with_lane_id.point.pose.position.y = target_point.y(); target_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; // Insert stop pose into path or replace with zero velocity - planning_utils::insertVelocity( - modified_path, target_point_with_lane_id, 0.0, target_velocity_point_idx); + size_t insert_index = insert_target_point_idx; + planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index); if (static_cast(target_velocity_point_idx) < first_stop_path_point_index_) { first_stop_path_point_index_ = static_cast(target_velocity_point_idx); debug_data_.first_stop_pose = target_point_with_lane_id.point.pose; From 0534cc08e3968256096b3619cbedcdc99e61ab30 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 16 Mar 2022 11:48:42 +0900 Subject: [PATCH 15/15] feat(planning_error_monitor): suppress error when no message was set. (#515) * feat(planning_error_monitor): supress error when no message was set. Signed-off-by: Takayuki Murooka * add empty commit Signed-off-by: Takayuki Murooka --- .../src/planning_error_monitor_node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/planning_error_monitor/src/planning_error_monitor_node.cpp b/planning/planning_error_monitor/src/planning_error_monitor_node.cpp index b686ce70a4b4..dd43bacfca57 100644 --- a/planning/planning_error_monitor/src/planning_error_monitor_node.cpp +++ b/planning/planning_error_monitor/src/planning_error_monitor_node.cpp @@ -74,6 +74,7 @@ void PlanningErrorMonitorNode::onCurrentTrajectory(const Trajectory::ConstShared void PlanningErrorMonitorNode::onTrajectoryPointValueChecker(DiagnosticStatusWrapper & stat) { if (!current_trajectory_) { + stat.summary(DiagnosticStatus::OK, "No trajectory message was set."); return; } @@ -87,6 +88,7 @@ void PlanningErrorMonitorNode::onTrajectoryPointValueChecker(DiagnosticStatusWra void PlanningErrorMonitorNode::onTrajectoryIntervalChecker(DiagnosticStatusWrapper & stat) { if (!current_trajectory_) { + stat.summary(DiagnosticStatus::OK, "No trajectory message was set."); return; } @@ -101,6 +103,7 @@ void PlanningErrorMonitorNode::onTrajectoryIntervalChecker(DiagnosticStatusWrapp void PlanningErrorMonitorNode::onTrajectoryCurvatureChecker(DiagnosticStatusWrapper & stat) { if (!current_trajectory_) { + stat.summary(DiagnosticStatus::OK, "No trajectory message was set."); return; } @@ -115,6 +118,7 @@ void PlanningErrorMonitorNode::onTrajectoryCurvatureChecker(DiagnosticStatusWrap void PlanningErrorMonitorNode::onTrajectoryRelativeAngleChecker(DiagnosticStatusWrapper & stat) { if (!current_trajectory_) { + stat.summary(DiagnosticStatus::OK, "No trajectory message was set."); return; }