From f1e7f07d348f0a9037b1f12527bd6fd7b92f05f8 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 12 May 2023 19:18:16 +0900 Subject: [PATCH 01/13] test(obstacle_avoidance_planner): add interface test (#3404) * add planning_interface_test_manager class Signed-off-by: kyoichi-sugahara * add counter function Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add interface test for motion_velocity_smoother Signed-off-by: kyoichi-sugahara * add param in default_motion_velocity_smoother,param.yaml Signed-off-by: kyoichi-sugahara * successfully launch target node Signed-off-by: kyoichi-sugahara * successfully build the test Signed-off-by: kyoichi-sugahara * add test for test_motion_velocity_smoother Signed-off-by: kyoichi-sugahara * add abnormal trajectory test Signed-off-by: kyoichi-sugahara * delete unnecessary part Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * style(pre-commit): autofix * run pre-commit Signed-off-by: kyoichi-sugahara * add declaration Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * Refactor callback functions for standardization Signed-off-by: kyoichi-sugahara * refacotoring Signed-off-by: kyoichi-sugahara * refactored Signed-off-by: kyoichi-sugahara * Refactor functions into a single template function Signed-off-by: kyoichi-sugahara * Refactor Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * refactor Signed-off-by: kyoichi-sugahara * relete unnecessary definition Signed-off-by: kyoichi-sugahara * Revert "delete unnecessary definition" This reverts commit 6cd13f82868f34140fa1538d43f99d99b9648df3. Signed-off-by: kyoichi-sugahara * delete unnecessary definition Signed-off-by: kyoichi-sugahara * delete unnecessary definition Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete module dependent part Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete unnecessary arg Signed-off-by: kyoichi-sugahara * apply motion_velocity_smoother change Signed-off-by: kyoichi-sugahara * add interface test for obstacle stop planner Signed-off-by: kyoichi-sugahara * run pre-commit Signed-off-by: kyoichi-sugahara * add test fot obstacle_cruise_planner Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add test fot planning_vaildator Signed-off-by: kyoichi-sugahara * add interface test for freespace_planner_node Signed-off-by: kyoichi-sugahara * add interface test for obstacle_stop_planner with slow down Signed-off-by: kyoichi-sugahara * add part of interface test for freespace Signed-off-by: kyoichi-sugahara * change package name Signed-off-by: kyoichi-sugahara * apply change of package name Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * fix build error Signed-off-by: kyoichi-sugahara * temp commit for debug Signed-off-by: kyoichi-sugahara * add planning interface test manager for scenario selector Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * update parameter Signed-off-by: kyoichi-sugahara * temp for behavior_path_planner Signed-off-by: kyoichi-sugahara * add param file Signed-off-by: kyoichi-sugahara * add test free space planner module Signed-off-by: kyoichi-sugahara * update map Signed-off-by: kyoichi-sugahara * add test for behavior path planner Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * pass freespace test Signed-off-by: kyoichi-sugahara * pass freespace test Signed-off-by: kyoichi-sugahara * update map Signed-off-by: kyoichi-sugahara * update map detection area Signed-off-by: kyoichi-sugahara * add no stopping area Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * for print debug Signed-off-by: kyoichi-sugahara * update param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add param file Signed-off-by: kyoichi-sugahara * refactor(behavior_path_planner): remove unnecessary functions (#3271) * refactor(behavior_path_planner): remove unnecessary functions Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka * Revert "temp" This reverts commit b82805e29744f7689885f15d0ce89e760c73a7b9. Signed-off-by: kyoichi-sugahara * suppress build error (-Werror=pedantic) Signed-off-by: kyoichi-sugahara * add interface test for behavior_velocity_planner * build success Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * pass test Signed-off-by: kyoichi-sugahara * pass test Signed-off-by: kyoichi-sugahara * build success Signed-off-by: kyoichi-sugahara * update param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete param file Signed-off-by: kyoichi-sugahara * Resolve differences outside of parameter files Signed-off-by: kyoichi-sugahara * update test manager Signed-off-by: kyoichi-sugahara * update test manager Signed-off-by: kyoichi-sugahara * upload cloud map Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara * refactor utils Signed-off-by: kyoichi-sugahara * refactor utils Signed-off-by: kyoichi-sugahara * refactor test_manager Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * modify test for obstacle cruise planner Signed-off-by: kyoichi-sugahara * modify test for obstacle_stop_planner Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * revert parameter change Signed-off-by: kyoichi-sugahara * add obstacle_avoidance_planner Signed-off-by: kyoichi-sugahara * Revert "add obstacle_avoidance_planner" This reverts commit efb5d50babe917cb3388e348f0f8a621b3969ffd. Signed-off-by: kyoichi-sugahara * add obstacle_avoidance_planner Signed-off-by: kyoichi-sugahara * feat(planning_test_utils): update test manager for all planning modules (#3364) * add planning_interface_test_manager class Signed-off-by: kyoichi-sugahara * add counter function Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add interface test for motion_velocity_smoother Signed-off-by: kyoichi-sugahara * add param in default_motion_velocity_smoother,param.yaml Signed-off-by: kyoichi-sugahara * successfully launch target node Signed-off-by: kyoichi-sugahara * successfully build the test Signed-off-by: kyoichi-sugahara * add test for test_motion_velocity_smoother Signed-off-by: kyoichi-sugahara * add abnormal trajectory test Signed-off-by: kyoichi-sugahara * delete unnecessary part Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * style(pre-commit): autofix * run pre-commit Signed-off-by: kyoichi-sugahara * add declaration Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * Refactor callback functions for standardization Signed-off-by: kyoichi-sugahara * refacotoring Signed-off-by: kyoichi-sugahara * refactored Signed-off-by: kyoichi-sugahara * Refactor functions into a single template function Signed-off-by: kyoichi-sugahara * Refactor Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * refactor Signed-off-by: kyoichi-sugahara * relete unnecessary definition Signed-off-by: kyoichi-sugahara * Revert "delete unnecessary definition" This reverts commit 6cd13f82868f34140fa1538d43f99d99b9648df3. Signed-off-by: kyoichi-sugahara * delete unnecessary definition Signed-off-by: kyoichi-sugahara * delete unnecessary definition Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete module dependent part Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete unnecessary arg Signed-off-by: kyoichi-sugahara * apply motion_velocity_smoother change Signed-off-by: kyoichi-sugahara * add interface test for obstacle stop planner Signed-off-by: kyoichi-sugahara * run pre-commit Signed-off-by: kyoichi-sugahara * add test fot obstacle_cruise_planner Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add test fot planning_vaildator Signed-off-by: kyoichi-sugahara * add interface test for freespace_planner_node Signed-off-by: kyoichi-sugahara * add interface test for obstacle_stop_planner with slow down Signed-off-by: kyoichi-sugahara * add part of interface test for freespace Signed-off-by: kyoichi-sugahara * change package name Signed-off-by: kyoichi-sugahara * apply change of package name Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * fix build error Signed-off-by: kyoichi-sugahara * temp commit for debug Signed-off-by: kyoichi-sugahara * add planning interface test manager for scenario selector Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * update parameter Signed-off-by: kyoichi-sugahara * temp for behavior_path_planner Signed-off-by: kyoichi-sugahara * add param file Signed-off-by: kyoichi-sugahara * add test free space planner module Signed-off-by: kyoichi-sugahara * update map Signed-off-by: kyoichi-sugahara * add test for behavior path planner Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * pass freespace test Signed-off-by: kyoichi-sugahara * pass freespace test Signed-off-by: kyoichi-sugahara * update map Signed-off-by: kyoichi-sugahara * update map detection area Signed-off-by: kyoichi-sugahara * add no stopping area Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * for print debug Signed-off-by: kyoichi-sugahara * update param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add param file Signed-off-by: kyoichi-sugahara * refactor(behavior_path_planner): remove unnecessary functions (#3271) * refactor(behavior_path_planner): remove unnecessary functions Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka * Revert "temp" This reverts commit b82805e29744f7689885f15d0ce89e760c73a7b9. Signed-off-by: kyoichi-sugahara * suppress build error (-Werror=pedantic) Signed-off-by: kyoichi-sugahara * add interface test for behavior_velocity_planner * build success Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * pass test Signed-off-by: kyoichi-sugahara * pass test Signed-off-by: kyoichi-sugahara * build success Signed-off-by: kyoichi-sugahara * update param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete param file Signed-off-by: kyoichi-sugahara * Resolve differences outside of parameter files Signed-off-by: kyoichi-sugahara * update test manager Signed-off-by: kyoichi-sugahara * update test manager Signed-off-by: kyoichi-sugahara * upload cloud map Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara * refactor utils Signed-off-by: kyoichi-sugahara * refactor utils Signed-off-by: kyoichi-sugahara * refactor test_manager Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * modify test for obstacle cruise planner Signed-off-by: kyoichi-sugahara * modify test for obstacle_stop_planner Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * revert parameter change Signed-off-by: kyoichi-sugahara * add obstacle_avoidance_planner Signed-off-by: kyoichi-sugahara * Revert "add obstacle_avoidance_planner" This reverts commit efb5d50babe917cb3388e348f0f8a621b3969ffd. Signed-off-by: kyoichi-sugahara * change file name Signed-off-by: kyoichi-sugahara * change file name Signed-off-by: kyoichi-sugahara * modify CMake Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Signed-off-by: yutaka Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Signed-off-by: kyoichi-sugahara * fix config name Signed-off-by: kyoichi-sugahara * delete unnecessary files Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * update test_manager Signed-off-by: kyoichi-sugahara * update test_manager Signed-off-by: kyoichi-sugahara * fix node name in commnet Signed-off-by: kyoichi-sugahara * fix build error Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Signed-off-by: yutaka Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> --- .../obstacle_avoidance_planner/CMakeLists.txt | 9 +++ .../obstacle_avoidance_planner/package.xml | 1 + ...tacle_avoidance_planner_node_interface.cpp | 65 +++++++++++++++++++ .../planning_interface_test_manager.hpp | 11 ++++ .../planning_interface_test_manager_utils.hpp | 14 ++++ .../src/planning_interface_test_manager.cpp | 30 +++++++++ 6 files changed, 130 insertions(+) create mode 100644 planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index c851de40aa9ec..399a596a0fb30 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -46,6 +46,15 @@ rclcpp_components_register_node(obstacle_avoidance_planner EXECUTABLE obstacle_avoidance_planner_node ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_${PROJECT_NAME}_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml index 5da94d1c25d74..81429368fc7e4 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/obstacle_avoidance_planner/package.xml @@ -20,6 +20,7 @@ motion_utils nav_msgs osqp_interface + planning_test_utils rclcpp rclcpp_components std_msgs diff --git a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp b/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp new file mode 100644 index 0000000000000..5ca4e2d29b912 --- /dev/null +++ b/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp @@ -0,0 +1,65 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "obstacle_avoidance_planner/node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + + auto test_manager = std::make_shared(); + + auto node_options = rclcpp::NodeOptions{}; + + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto obstacle_avoidance_planner_dir = + ament_index_cpp::get_package_share_directory("obstacle_avoidance_planner"); + + node_options.arguments( + {"--ros-args", "--params-file", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", + planning_test_utils_dir + "/config/test_common.param.yaml", "--params-file", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", + obstacle_avoidance_planner_dir + "/config/obstacle_avoidance_planner.param.yaml"}); + + auto test_target_node = + std::make_shared(node_options); + + // publish necessary topics from test_manager + test_manager->publishOdometry(test_target_node, "obstacle_avoidance_planner/input/odometry"); + + // set subscriber with topic name: obstacle_avoidance_planner → test_node_ + test_manager->setTrajectorySubscriber("obstacle_avoidance_planner/output/path"); + + // set obstacle_avoidance_planner's input topic name(this topic is changed to test node) + test_manager->setPathInputTopicName("obstacle_avoidance_planner/input/path"); + + // test with normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalPath(test_target_node)); + + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testWithAbnormalPath(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index 54407df14aa78..fe029029c03f3 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -126,6 +126,7 @@ class PlanningInterfaceTestManager void setRouteInputTopicName(std::string topic_name); void setPathInputTopicName(std::string topic_name); void setPathWithLaneIdTopicName(std::string topic_name); + void setPathTopicName(std::string topic_name); void setTrajectorySubscriber(std::string topic_name); void setScenarioSubscriber(std::string topic_name); @@ -147,6 +148,9 @@ class PlanningInterfaceTestManager void testWithNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node); void testWithAbnormalPathWithLaneId(rclcpp::Node::SharedPtr target_node); + void testWithNominalPath(rclcpp::Node::SharedPtr target_node); + void testWithAbnormalPath(rclcpp::Node::SharedPtr target_node); + // for invalid ego poses, contains some tests inside. void testRouteWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); void testPathWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node); @@ -211,6 +215,10 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr normal_path_with_lane_id_pub_; rclcpp::Publisher::SharedPtr abnormal_path_with_lane_id_pub_; + // Publisher for testing(Path) + rclcpp::Publisher::SharedPtr normal_path_pub_; + rclcpp::Publisher::SharedPtr abnormal_path_pub_; + std::string input_trajectory_name_ = ""; std::string input_parking_trajectory_name_ = ""; std::string input_lane_driving_trajectory_name_ = ""; @@ -239,6 +247,9 @@ class PlanningInterfaceTestManager void publishBehaviorNominalRoute(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishAbNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node, std::string topic_name); + + void publishNominalPath(rclcpp::Node::SharedPtr target_node, std::string topic_name); + void publishAbnormalPath(rclcpp::Node::SharedPtr target_node, std::string topic_name); }; // class PlanningInterfaceTestManager } // namespace planning_test_utils diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp index 6e281dfed73fb..685ca5f69d404 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp @@ -56,6 +56,7 @@ namespace test_utils { using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; @@ -383,6 +384,19 @@ void updateNodeOptions( node_options.arguments(std::vector{arguments.begin(), arguments.end()}); } +Path toPath(const PathWithLaneId & input) +{ + Path output{}; + output.header = input.header; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; + output.points.resize(input.points.size()); + for (size_t i = 0; i < input.points.size(); ++i) { + output.points.at(i) = input.points.at(i).point; + } + return output; +} + PathWithLaneId loadPathWithLaneIdInYaml() { const auto planning_test_utils_dir = diff --git a/planning/planning_test_utils/src/planning_interface_test_manager.cpp b/planning/planning_test_utils/src/planning_interface_test_manager.cpp index 84a4da0b8e7b0..f57c3a45b79c6 100644 --- a/planning/planning_test_utils/src/planning_interface_test_manager.cpp +++ b/planning/planning_test_utils/src/planning_interface_test_manager.cpp @@ -217,6 +217,11 @@ void PlanningInterfaceTestManager::setRouteInputTopicName(std::string topic_name input_route_name_ = topic_name; } +void PlanningInterfaceTestManager::setPathInputTopicName(std::string topic_name) +{ + input_path_name_ = topic_name; +} + void PlanningInterfaceTestManager::setPathWithLaneIdTopicName(std::string topic_name) { input_path_with_lane_id_name_ = topic_name; @@ -284,6 +289,21 @@ void PlanningInterfaceTestManager::publishAbNominalPathWithLaneId( test_node_, target_node, topic_name, abnormal_path_with_lane_id_pub_, PathWithLaneId{}, 5); } +void PlanningInterfaceTestManager::publishNominalPath( + rclcpp::Node::SharedPtr target_node, std::string topic_name) +{ + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, normal_path_pub_, + test_utils::toPath(test_utils::loadPathWithLaneIdInYaml()), 5); +} + +void PlanningInterfaceTestManager::publishAbnormalPath( + rclcpp::Node::SharedPtr target_node, std::string topic_name) +{ + test_utils::publishToTargetNode( + test_node_, target_node, topic_name, abnormal_path_pub_, Path{}, 5); +} + void PlanningInterfaceTestManager::setTrajectorySubscriber(std::string topic_name) { test_utils::setSubscriber(test_node_, topic_name, traj_sub_, count_); @@ -418,6 +438,16 @@ void PlanningInterfaceTestManager::testOffTrackFromTrajectory(rclcpp::Node::Shar } } +void PlanningInterfaceTestManager::testWithNominalPath(rclcpp::Node::SharedPtr target_node) +{ + publishNominalPath(target_node, input_path_name_); +} + +void PlanningInterfaceTestManager::testWithAbnormalPath(rclcpp::Node::SharedPtr target_node) +{ + publishAbnormalPath(target_node, input_path_name_); +} + int PlanningInterfaceTestManager::getReceivedTopicNum() { return count_; From 1f9b1371f52a473a3d52ed2b8fe6cd25a77e0989 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 12 May 2023 21:29:10 +0900 Subject: [PATCH 02/13] test(behavior_velocity_planner): add interface test (#3401) * add planning_interface_test_manager class Signed-off-by: kyoichi-sugahara * add counter function Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add interface test for motion_velocity_smoother Signed-off-by: kyoichi-sugahara * add param in default_motion_velocity_smoother,param.yaml Signed-off-by: kyoichi-sugahara * successfully launch target node Signed-off-by: kyoichi-sugahara * successfully build the test Signed-off-by: kyoichi-sugahara * add test for test_motion_velocity_smoother Signed-off-by: kyoichi-sugahara * add abnormal trajectory test Signed-off-by: kyoichi-sugahara * delete unnecessary part Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * style(pre-commit): autofix * run pre-commit Signed-off-by: kyoichi-sugahara * add declaration Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * Refactor callback functions for standardization Signed-off-by: kyoichi-sugahara * refacotoring Signed-off-by: kyoichi-sugahara * refactored Signed-off-by: kyoichi-sugahara * Refactor functions into a single template function Signed-off-by: kyoichi-sugahara * Refactor Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * refactor Signed-off-by: kyoichi-sugahara * relete unnecessary definition Signed-off-by: kyoichi-sugahara * Revert "delete unnecessary definition" This reverts commit 6cd13f82868f34140fa1538d43f99d99b9648df3. Signed-off-by: kyoichi-sugahara * delete unnecessary definition Signed-off-by: kyoichi-sugahara * delete unnecessary definition Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete module dependent part Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete unnecessary arg Signed-off-by: kyoichi-sugahara * apply motion_velocity_smoother change Signed-off-by: kyoichi-sugahara * add interface test for obstacle stop planner Signed-off-by: kyoichi-sugahara * run pre-commit Signed-off-by: kyoichi-sugahara * add test fot obstacle_cruise_planner Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add test fot planning_vaildator Signed-off-by: kyoichi-sugahara * add interface test for freespace_planner_node Signed-off-by: kyoichi-sugahara * add interface test for obstacle_stop_planner with slow down Signed-off-by: kyoichi-sugahara * add part of interface test for freespace Signed-off-by: kyoichi-sugahara * change package name Signed-off-by: kyoichi-sugahara * apply change of package name Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * fix build error Signed-off-by: kyoichi-sugahara * temp commit for debug Signed-off-by: kyoichi-sugahara * add planning interface test manager for scenario selector Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * update parameter Signed-off-by: kyoichi-sugahara * temp for behavior_path_planner Signed-off-by: kyoichi-sugahara * add param file Signed-off-by: kyoichi-sugahara * add test free space planner module Signed-off-by: kyoichi-sugahara * update map Signed-off-by: kyoichi-sugahara * add test for behavior path planner Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * pass freespace test Signed-off-by: kyoichi-sugahara * pass freespace test Signed-off-by: kyoichi-sugahara * update map Signed-off-by: kyoichi-sugahara * update map detection area Signed-off-by: kyoichi-sugahara * add no stopping area Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * for print debug Signed-off-by: kyoichi-sugahara * update param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add param file Signed-off-by: kyoichi-sugahara * refactor(behavior_path_planner): remove unnecessary functions (#3271) * refactor(behavior_path_planner): remove unnecessary functions Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka * Revert "temp" This reverts commit b82805e29744f7689885f15d0ce89e760c73a7b9. Signed-off-by: kyoichi-sugahara * suppress build error (-Werror=pedantic) Signed-off-by: kyoichi-sugahara * add interface test for behavior_velocity_planner * build success Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * pass test Signed-off-by: kyoichi-sugahara * pass test Signed-off-by: kyoichi-sugahara * build success Signed-off-by: kyoichi-sugahara * update param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete param file Signed-off-by: kyoichi-sugahara * Resolve differences outside of parameter files Signed-off-by: kyoichi-sugahara * update test manager Signed-off-by: kyoichi-sugahara * update test manager Signed-off-by: kyoichi-sugahara * upload cloud map Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara * refactor utils Signed-off-by: kyoichi-sugahara * refactor utils Signed-off-by: kyoichi-sugahara * refactor test_manager Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * modify test for obstacle cruise planner Signed-off-by: kyoichi-sugahara * modify test for obstacle_stop_planner Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * revert parameter change Signed-off-by: kyoichi-sugahara * add obstacle_avoidance_planner Signed-off-by: kyoichi-sugahara * Revert "add obstacle_avoidance_planner" This reverts commit efb5d50babe917cb3388e348f0f8a621b3969ffd. Signed-off-by: kyoichi-sugahara * add obstacle_avoidance_planner Signed-off-by: kyoichi-sugahara * change file name Signed-off-by: kyoichi-sugahara * change file name Signed-off-by: kyoichi-sugahara * modify CMake Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * build success Signed-off-by: kyoichi-sugahara * test success Signed-off-by: kyoichi-sugahara * execption test is added Signed-off-by: kyoichi-sugahara * delete unnecessary debug code Signed-off-by: kyoichi-sugahara * add prototype declaration Signed-off-by: kyoichi-sugahara * delete unnecessary comments Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * feat(planning_test_utils): update test manager for all planning modules (#3364) * add planning_interface_test_manager class Signed-off-by: kyoichi-sugahara * add counter function Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add interface test for motion_velocity_smoother Signed-off-by: kyoichi-sugahara * add param in default_motion_velocity_smoother,param.yaml Signed-off-by: kyoichi-sugahara * successfully launch target node Signed-off-by: kyoichi-sugahara * successfully build the test Signed-off-by: kyoichi-sugahara * add test for test_motion_velocity_smoother Signed-off-by: kyoichi-sugahara * add abnormal trajectory test Signed-off-by: kyoichi-sugahara * delete unnecessary part Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * style(pre-commit): autofix * run pre-commit Signed-off-by: kyoichi-sugahara * add declaration Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * Refactor callback functions for standardization Signed-off-by: kyoichi-sugahara * refacotoring Signed-off-by: kyoichi-sugahara * refactored Signed-off-by: kyoichi-sugahara * Refactor functions into a single template function Signed-off-by: kyoichi-sugahara * Refactor Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * refactor Signed-off-by: kyoichi-sugahara * relete unnecessary definition Signed-off-by: kyoichi-sugahara * Revert "delete unnecessary definition" This reverts commit 6cd13f82868f34140fa1538d43f99d99b9648df3. Signed-off-by: kyoichi-sugahara * delete unnecessary definition Signed-off-by: kyoichi-sugahara * delete unnecessary definition Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete module dependent part Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete unnecessary arg Signed-off-by: kyoichi-sugahara * apply motion_velocity_smoother change Signed-off-by: kyoichi-sugahara * add interface test for obstacle stop planner Signed-off-by: kyoichi-sugahara * run pre-commit Signed-off-by: kyoichi-sugahara * add test fot obstacle_cruise_planner Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add test fot planning_vaildator Signed-off-by: kyoichi-sugahara * add interface test for freespace_planner_node Signed-off-by: kyoichi-sugahara * add interface test for obstacle_stop_planner with slow down Signed-off-by: kyoichi-sugahara * add part of interface test for freespace Signed-off-by: kyoichi-sugahara * change package name Signed-off-by: kyoichi-sugahara * apply change of package name Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * fix build error Signed-off-by: kyoichi-sugahara * temp commit for debug Signed-off-by: kyoichi-sugahara * add planning interface test manager for scenario selector Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * update parameter Signed-off-by: kyoichi-sugahara * temp for behavior_path_planner Signed-off-by: kyoichi-sugahara * add param file Signed-off-by: kyoichi-sugahara * add test free space planner module Signed-off-by: kyoichi-sugahara * update map Signed-off-by: kyoichi-sugahara * add test for behavior path planner Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * pass freespace test Signed-off-by: kyoichi-sugahara * pass freespace test Signed-off-by: kyoichi-sugahara * update map Signed-off-by: kyoichi-sugahara * update map detection area Signed-off-by: kyoichi-sugahara * add no stopping area Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * for print debug Signed-off-by: kyoichi-sugahara * update param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add param file Signed-off-by: kyoichi-sugahara * refactor(behavior_path_planner): remove unnecessary functions (#3271) * refactor(behavior_path_planner): remove unnecessary functions Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka * Revert "temp" This reverts commit b82805e29744f7689885f15d0ce89e760c73a7b9. Signed-off-by: kyoichi-sugahara * suppress build error (-Werror=pedantic) Signed-off-by: kyoichi-sugahara * add interface test for behavior_velocity_planner * build success Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * add param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * pass test Signed-off-by: kyoichi-sugahara * pass test Signed-off-by: kyoichi-sugahara * build success Signed-off-by: kyoichi-sugahara * update param Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * delete param file Signed-off-by: kyoichi-sugahara * Resolve differences outside of parameter files Signed-off-by: kyoichi-sugahara * update test manager Signed-off-by: kyoichi-sugahara * update test manager Signed-off-by: kyoichi-sugahara * upload cloud map Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara * refactor utils Signed-off-by: kyoichi-sugahara * refactor utils Signed-off-by: kyoichi-sugahara * refactor test_manager Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * modify test for obstacle cruise planner Signed-off-by: kyoichi-sugahara * modify test for obstacle_stop_planner Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * revert parameter change Signed-off-by: kyoichi-sugahara * add obstacle_avoidance_planner Signed-off-by: kyoichi-sugahara * Revert "add obstacle_avoidance_planner" This reverts commit efb5d50babe917cb3388e348f0f8a621b3969ffd. Signed-off-by: kyoichi-sugahara * change file name Signed-off-by: kyoichi-sugahara * change file name Signed-off-by: kyoichi-sugahara * modify CMake Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara * refactor Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Signed-off-by: yutaka Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Signed-off-by: kyoichi-sugahara * fix config name Signed-off-by: kyoichi-sugahara * delete unnecessary files Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * temp Signed-off-by: kyoichi-sugahara * update test_manager Signed-off-by: kyoichi-sugahara * update test_manager Signed-off-by: kyoichi-sugahara * fix node name in commnet Signed-off-by: kyoichi-sugahara * fix build error Signed-off-by: kyoichi-sugahara * fix build error Signed-off-by: kyoichi-sugahara * delete unnecessary publisher from test_node Signed-off-by: kyoichi-sugahara * set flag for launch scene module Signed-off-by: kyoichi-sugahara * add ego pose test with comment out Signed-off-by: kyoichi-sugahara * add username in TODO Signed-off-by: kyoichi-sugahara * enable off-track test Signed-off-by: Takamasa Horibe --------- Signed-off-by: kyoichi-sugahara Signed-off-by: yutaka Signed-off-by: Takamasa Horibe Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Takamasa Horibe --- .../behavior_velocity_planner/CMakeLists.txt | 8 + ...havior_velocity_planner_node_interface.cpp | 156 ++++++++++++++++++ .../planning_interface_test_manager.hpp | 7 - .../src/planning_interface_test_manager.cpp | 18 -- 4 files changed, 164 insertions(+), 25 deletions(-) create mode 100644 planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 870791444adf3..1c97d8c27c934 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -87,6 +87,14 @@ if(BUILD_TESTING) gtest_main behavior_velocity_planner ) + + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_${PROJECT_NAME}_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp new file mode 100644 index 0000000000000..6d251aee70677 --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp @@ -0,0 +1,156 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_velocity_planner/node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_velocity_planner::BehaviorVelocityPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_velocity_planner → test_node_ + test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path"); + + // set behavior_velocity_planner node's input topic name(this topic is changed to test node) + test_manager->setPathWithLaneIdTopicName( + "behavior_velocity_planner_node/input/path_with_lane_id"); + + test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_velocity_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_velocity_planner"); + const auto motion_velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", + motion_velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", + behavior_velocity_planner_dir + "/config/blind_spot.param.yaml", + behavior_velocity_planner_dir + "/config/crosswalk.param.yaml", + behavior_velocity_planner_dir + "/config/detection_area.param.yaml", + behavior_velocity_planner_dir + "/config/intersection.param.yaml", + behavior_velocity_planner_dir + "/config/no_stopping_area.param.yaml", + behavior_velocity_planner_dir + "/config/occlusion_spot.param.yaml", + behavior_velocity_planner_dir + "/config/run_out.param.yaml", + behavior_velocity_planner_dir + "/config/speed_bump.param.yaml", + behavior_velocity_planner_dir + "/config/stop_line.param.yaml", + behavior_velocity_planner_dir + "/config/traffic_light.param.yaml", + behavior_velocity_planner_dir + "/config/virtual_traffic_light.param.yaml", + behavior_velocity_planner_dir + "/config/out_of_lane.param.yaml"}); + + node_options.append_parameter_override("launch_stop_line", true); + node_options.append_parameter_override("launch_crosswalk", true); + node_options.append_parameter_override("launch_traffic_light", true); + node_options.append_parameter_override("launch_intersection", true); + node_options.append_parameter_override("launch_blind_spot", true); + node_options.append_parameter_override("launch_detection_area", true); + node_options.append_parameter_override( + "launch_virtual_traffic_light", false); // TODO(Kyoichi Sugahara) set to true + node_options.append_parameter_override( + "launch_occlusion_spot", false); // TODO(Kyoichi Sugahara) set to true + node_options.append_parameter_override("launch_no_stopping_area", true); + node_options.append_parameter_override( + "launch_run_out", false); // TODO(Kyoichi Sugahara) set to true + node_options.append_parameter_override( + "launch_speed_bump", false); // TODO(Kyoichi Sugahara) set to true + node_options.append_parameter_override("launch_out_of_lane", true); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishTF(test_target_node, "/tf"); + test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); + test_manager->publishPredictedObjects( + test_target_node, "behavior_velocity_planner_node/input/dynamic_objects"); + test_manager->publishPointCloud( + test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud"); + test_manager->publishOdometry( + test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); + test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map"); + test_manager->publishTrafficSignals( + test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); + test_manager->publishMaxVelocity( + test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); + test_manager->publishVirtualTrafficLightState( + test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index fe029029c03f3..9280aad2cce7a 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -114,11 +114,7 @@ class PlanningInterfaceTestManager void publishLateralOffset(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishOperationModeState(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishExternalTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishVirtualTrafficLightState(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishExternalCrosswalkStates(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishExternalIntersectionStates( - rclcpp::Node::SharedPtr target_node, std::string topic_name); void setTrajectoryInputTopicName(std::string topic_name); void setParkingTrajectoryInputTopicName(std::string topic_name); @@ -188,10 +184,7 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr lateral_offset_pub_; rclcpp::Publisher::SharedPtr operation_mode_state_pub_; rclcpp::Publisher::SharedPtr traffic_signals_pub_; - rclcpp::Publisher::SharedPtr external_traffic_signals_pub_; rclcpp::Publisher::SharedPtr virtual_traffic_light_states_pub_; - rclcpp::Publisher::SharedPtr external_crosswalk_states_pub_; - rclcpp::Publisher::SharedPtr external_intersection_states_pub_; // Subscriber rclcpp::Subscription::SharedPtr traj_sub_; diff --git a/planning/planning_test_utils/src/planning_interface_test_manager.cpp b/planning/planning_test_utils/src/planning_interface_test_manager.cpp index f57c3a45b79c6..a0b887fc4d089 100644 --- a/planning/planning_test_utils/src/planning_interface_test_manager.cpp +++ b/planning/planning_test_utils/src/planning_interface_test_manager.cpp @@ -163,12 +163,6 @@ void PlanningInterfaceTestManager::publishTrafficSignals( test_node_, target_node, topic_name, traffic_signals_pub_, TrafficSignalArray{}); } -void PlanningInterfaceTestManager::publishExternalTrafficSignals( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - test_utils::publishToTargetNode( - test_node_, target_node, topic_name, external_traffic_signals_pub_, TrafficSignalArray{}); -} void PlanningInterfaceTestManager::publishVirtualTrafficLightState( rclcpp::Node::SharedPtr target_node, std::string topic_name) { @@ -176,18 +170,6 @@ void PlanningInterfaceTestManager::publishVirtualTrafficLightState( test_node_, target_node, topic_name, virtual_traffic_light_states_pub_, VirtualTrafficLightStateArray{}); } -void PlanningInterfaceTestManager::publishExternalCrosswalkStates( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - test_utils::publishToTargetNode( - test_node_, target_node, topic_name, external_crosswalk_states_pub_, CrosswalkStatus{}); -} -void PlanningInterfaceTestManager::publishExternalIntersectionStates( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - test_utils::publishToTargetNode( - test_node_, target_node, topic_name, external_intersection_states_pub_, IntersectionStatus{}); -} void PlanningInterfaceTestManager::publishInitialPoseTF( rclcpp::Node::SharedPtr target_node, std::string topic_name) From 1b464d6f93250bcca9660b7d5218939b015e77db Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 12 May 2023 14:18:19 +0900 Subject: [PATCH 03/13] fix(multi_object_tracker): fix unintended perturbation in tracking estimation via non initialized eigen vector (#3651) (#423) add initialize for anchor point vector Signed-off-by: yoshiri Co-authored-by: Yoshi Ri --- .../src/tracker/model/big_vehicle_tracker.cpp | 3 ++- .../src/tracker/model/normal_vehicle_tracker.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index c147035345901..458509cf06e86 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -44,7 +44,8 @@ BigVehicleTracker::BigVehicleTracker( : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), last_update_time_(time), - z_(object.kinematics.pose_with_covariance.pose.position.z) + z_(object.kinematics.pose_with_covariance.pose.position.z), + tracking_offset_(Eigen::Vector2d::Zero()) { object_ = object; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index f2a8488ec6089..000f728892d11 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -44,7 +44,8 @@ NormalVehicleTracker::NormalVehicleTracker( : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), last_update_time_(time), - z_(object.kinematics.pose_with_covariance.pose.position.z) + z_(object.kinematics.pose_with_covariance.pose.position.z), + tracking_offset_(Eigen::Vector2d::Zero()) { object_ = object; From 19071b9c30d924765cfa5e6a5fe2f487d86ba13f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 12 May 2023 20:10:42 +0900 Subject: [PATCH 04/13] fix(lane_change): safety check not working when cancel disabled (#3685) * fix(lane_change): safety check not working when cancel disabled Signed-off-by: Muhammad Zulfaqar * remove header Signed-off-by: Muhammad Zulfaqar * override updateState for new and old arch. Signed-off-by: Muhammad Zulfaqar * Revert "override updateState for new and old arch." This reverts commit 4c509e3a69f68c14d1bbf711818bc070bc9a9a90. --------- Signed-off-by: Muhammad Zulfaqar --- .../scene_module/lane_change/base_class.hpp | 2 ++ .../src/scene_module/lane_change/interface.cpp | 3 ++- .../src/scene_module/lane_change/normal.cpp | 4 ---- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index ad2bb9cdafe70..eee7bf9987a89 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -156,6 +156,8 @@ class LaneChangeBase bool isValidPath() const { return status_.is_valid_path; } + bool isCancelEnabled() const { return lane_change_parameters_->enable_cancel_lane_change; } + std_msgs::msg::Header getRouteHeader() const { return planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index a407a872d4279..1a9dccea104be 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -97,7 +97,8 @@ ModuleStatus LaneChangeInterface::updateState() } if (module_type_->isCancelConditionSatisfied()) { - current_state_ = ModuleStatus::FAILURE; + current_state_ = + module_type_->isCancelEnabled() ? ModuleStatus::FAILURE : ModuleStatus::RUNNING; return current_state_; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 19d612f2bc671..b04c33ea3263e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -163,10 +163,6 @@ bool NormalLaneChange::isCancelConditionSatisfied() { current_lane_change_state_ = LaneChangeStates::Normal; - if (!lane_change_parameters_->enable_cancel_lane_change) { - return false; - } - Pose ego_pose_before_collision; const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); From d6a5e0f4b3b9265770bb8966c7a0435f008542e7 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 15 May 2023 09:28:48 +0900 Subject: [PATCH 05/13] fix(lane_change): not remove unsafe candidate in new architecture (#3694) * fix(lane_change): not remove unsafe candidate in new architecture Signed-off-by: Muhammad Zulfaqar * By pass current_state_ check in isExecutionRequested Signed-off-by: Muhammad Zulfaqar * Compile with old architecture Signed-off-by: Muhammad Zulfaqar --------- Signed-off-by: Muhammad Zulfaqar --- .../scene_module/lane_change/interface.hpp | 2 + .../scene_module/lane_change/interface.cpp | 39 ++++++++++++++++--- 2 files changed, 35 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index 653c95b060163..0a04916170961 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -168,6 +168,8 @@ class LaneChangeBTModule : public LaneChangeBTInterface const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters); + ModuleStatus updateState() override; + protected: void updateRTCStatus(const double start_distance, const double finish_distance) override; }; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 1a9dccea104be..65cab5df809c8 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_planner/module_status.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" @@ -72,10 +73,6 @@ bool LaneChangeInterface::isExecutionRequested() const bool LaneChangeInterface::isExecutionReady() const { - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - LaneChangePath selected_path; module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); @@ -97,8 +94,11 @@ ModuleStatus LaneChangeInterface::updateState() } if (module_type_->isCancelConditionSatisfied()) { - current_state_ = - module_type_->isCancelEnabled() ? ModuleStatus::FAILURE : ModuleStatus::RUNNING; + if (module_type_->isCancelEnabled()) { + current_state_ = isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::FAILURE; + } else { + current_state_ = ModuleStatus::RUNNING; + } return current_state_; } @@ -474,6 +474,33 @@ LaneChangeBTModule::LaneChangeBTModule( { } +ModuleStatus LaneChangeBTModule::updateState() +{ + if (!module_type_->isValidPath()) { + current_state_ = ModuleStatus::FAILURE; + return current_state_; + } + + if (module_type_->isAbortState()) { + current_state_ = ModuleStatus::RUNNING; + return current_state_; + } + + if (module_type_->isCancelConditionSatisfied()) { + current_state_ = + module_type_->isCancelEnabled() ? ModuleStatus::FAILURE : ModuleStatus::RUNNING; + return current_state_; + } + + if (module_type_->hasFinishedLaneChange()) { + current_state_ = ModuleStatus::SUCCESS; + return current_state_; + } + + current_state_ = ModuleStatus::RUNNING; + return current_state_; +} + void LaneChangeBTModule::updateRTCStatus(const double start_distance, const double finish_distance) { const auto direction = std::invoke([&]() -> std::string { From 9151264eb2d6abaeb027170ccda54becd5f2f2d8 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 12 May 2023 18:56:12 +0900 Subject: [PATCH 06/13] feat(intersection): switch default/occlusion_first_stop when occlusion appeared/disappeared or approved/disapproved (#3664) Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 12 +---- .../src/scene_module/intersection/debug.cpp | 2 +- .../src/scene_module/intersection/manager.cpp | 46 ++++--------------- .../intersection/scene_intersection.cpp | 31 ++++++------- 4 files changed, 25 insertions(+), 66 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index e3a6f8ee6c809..ec152a075e6b8 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -150,15 +150,9 @@ class IntersectionModule : public SceneModuleInterface UUID getOcclusionUUID() const { return occlusion_uuid_; } bool getOcclusionSafety() const { return occlusion_safety_; } double getOcclusionDistance() const { return occlusion_stop_distance_; } - UUID getOcclusionFirstStopUUID() const { return occlusion_first_stop_uuid_; } - bool getOcclusionFirstStopSafety() const { return occlusion_first_stop_safety_; } - double getOcclusionFirstStopDistance() const { return occlusion_first_stop_distance_; } void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; } - void setOcclusionFirstStopActivation(const bool activation) - { - occlusion_first_stop_activated_ = activation; - } bool isOccluded() const { return is_actually_occluded_ || is_forcefully_occluded_; } + bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; } private: rclcpp::Node & node_; @@ -186,9 +180,7 @@ class IntersectionModule : public SceneModuleInterface bool occlusion_activated_ = true; // for first stop in two-phase stop const UUID occlusion_first_stop_uuid_; - bool occlusion_first_stop_safety_ = true; - double occlusion_first_stop_distance_; - bool occlusion_first_stop_activated_ = true; + bool occlusion_first_stop_required_ = false; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index fca5f73b608e2..0723d786f0d40 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -210,7 +210,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker {debug_data_.collision_stop_wall_pose}, "intersection", now), &wall_marker, now); } - if (!occlusion_first_stop_activated_) { + if (!activated_ && occlusion_first_stop_required_) { appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( {debug_data_.occlusion_first_stop_wall_pose}, "intersection", now), diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index e64e26c1555d7..b8f7f42414655 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -183,32 +183,17 @@ bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegister void IntersectionModuleManager::sendRTC(const Time & stamp) { - rtc_interface_.clearCooperateStatus(); for (const auto & scene_module : scene_modules_) { const auto intersection_module = std::dynamic_pointer_cast(scene_module); - const bool is_occluded = intersection_module->isOccluded(); const UUID uuid = getUUID(scene_module->getModuleId()); + const bool safety = + scene_module->isSafe() && (!intersection_module->isOcclusionFirstStopRequired()); + updateRTCStatus(uuid, safety, scene_module->getDistance(), stamp); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); const auto occlusion_distance = intersection_module->getOcclusionDistance(); - const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID(); - if (!is_occluded) { - // default - updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); - occlusion_rtc_interface_.updateCooperateStatus( - occlusion_uuid, true, occlusion_distance, occlusion_distance, stamp); - } else { - // occlusion - const auto occlusion_safety = intersection_module->getOcclusionSafety(); - occlusion_rtc_interface_.updateCooperateStatus( - occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); - - const auto occlusion_first_stop_safety = intersection_module->getOcclusionFirstStopSafety(); - const auto occlusion_first_stop_distance = - intersection_module->getOcclusionFirstStopDistance(); - rtc_interface_.updateCooperateStatus( - occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance, - occlusion_first_stop_distance, stamp); - } + const auto occlusion_safety = intersection_module->getOcclusionSafety(); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); } rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() occlusion_rtc_interface_.publishCooperateStatus(stamp); @@ -219,20 +204,9 @@ void IntersectionModuleManager::setActivation() for (const auto & scene_module : scene_modules_) { const auto intersection_module = std::dynamic_pointer_cast(scene_module); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); - const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID(); - const bool is_occluded = intersection_module->isOccluded(); - if (!is_occluded) { - // default - scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); - intersection_module->setOcclusionActivation( - occlusion_rtc_interface_.isActivated(occlusion_uuid)); - } else { - // occlusion - intersection_module->setOcclusionActivation( - occlusion_rtc_interface_.isActivated(occlusion_uuid)); - intersection_module->setOcclusionFirstStopActivation( - rtc_interface_.isActivated(occlusion_first_stop_uuid)); - } + scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); + intersection_module->setOcclusionActivation( + occlusion_rtc_interface_.isActivated(occlusion_uuid)); } } @@ -253,9 +227,7 @@ void IntersectionModuleManager::deleteExpiredModules( // occlusion const auto intersection_module = std::dynamic_pointer_cast(scene_module); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); - const auto occlusion_first_uuid = intersection_module->getOcclusionFirstStopUUID(); occlusion_rtc_interface_.removeCooperateStatus(occlusion_uuid); - rtc_interface_.removeCooperateStatus(occlusion_first_uuid); unregisterModule(scene_module); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 206d3341dc29a..d005243566ee7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -70,8 +70,7 @@ IntersectionModule::IntersectionModule( assoc_ids_(assoc_ids), enable_occlusion_detection_(enable_occlusion_detection), detection_divisions_(std::nullopt), - occlusion_uuid_(tier4_autoware_utils::generateUUID()), - occlusion_first_stop_uuid_(tier4_autoware_utils::generateUUID()) + occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(VelocityFactor::INTERSECTION); planner_param_ = planner_param; @@ -104,8 +103,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // occlusion occlusion_safety_ = true; occlusion_stop_distance_ = std::numeric_limits::lowest(); - occlusion_first_stop_safety_ = true; - occlusion_first_stop_distance_ = std::numeric_limits::lowest(); + occlusion_first_stop_required_ = false; /* get current pose */ const geometry_msgs::msg::Pose current_pose = planner_data_->current_odometry->pose; @@ -266,7 +264,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* calculate final stop lines */ std::optional stop_line_idx = default_stop_line_idx_opt; std::optional occlusion_peeking_line_idx = - occlusion_peeking_line_idx_opt; // TODO(Mamoru Sobue): different position depending on the flag + occlusion_peeking_line_idx_opt + ? std::make_optional(occlusion_peeking_line_idx_opt.value()) + : std::nullopt; std::optional occlusion_first_stop_line_idx = default_stop_line_idx_opt; std::optional> insert_creep_during_occlusion = std::nullopt; @@ -299,11 +299,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; } - if ( - before_creep_state_machine_.getState() == StateMachine::State::GO && - !ext_occlusion_requested) { + if (before_creep_state_machine_.getState() == StateMachine::State::GO) { occlusion_stop_required = true; - stop_line_idx = occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt; + occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt; + // clear first stop line // insert creep velocity [closest_idx, occlusion_stop_line) insert_creep_during_occlusion = @@ -311,7 +310,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE; } else { const bool approached_stop_line = - (dist_1st_stopline < planner_param_.common.stop_overshoot_margin); + (std::fabs(dist_1st_stopline) < planner_param_.common.stop_overshoot_margin); const bool is_stopped = planner_data_->isVehicleStopped(); if (is_stopped && approached_stop_line) { // start waiting at the first stop line @@ -387,13 +386,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * logger_.get_child("collision state_machine"), *clock_); /* set RTC safety respectively */ - occlusion_first_stop_distance_ = dist_1st_stopline; occlusion_stop_distance_ = dist_2nd_stopline; setDistance(dist_1st_stopline); if (occlusion_stop_required) { - if (first_phase_stop_required) { - occlusion_first_stop_safety_ = false; - } + occlusion_first_stop_required_ = first_phase_stop_required; occlusion_safety_ = is_occlusion_cleared; } else { /* collision */ @@ -414,7 +410,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } } - if (!occlusion_first_stop_activated_ && occlusion_first_stop_line_idx) { + if (!isActivated() && occlusion_first_stop_required_ && occlusion_first_stop_line_idx) { planning_utils::setVelocityFromIndex( occlusion_first_stop_line_idx.value(), 0.0 /* [m/s] */, path); debug_data_.occlusion_first_stop_wall_pose = @@ -427,10 +423,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_.occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_peeking_line_idx.value(), baselink2front, *path); } - - RCLCPP_DEBUG(logger_, "not activated. stop at the line."); RCLCPP_DEBUG(logger_, "===== plan end ====="); - return true; } if (!isActivated() /* collision*/) { @@ -455,6 +448,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); } + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return true; } is_go_out_ = true; From e99f5d7f73282bfa842e22da76d545e2015c0880 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 15 May 2023 17:38:56 +0900 Subject: [PATCH 07/13] fix(intersection): fixed bad optional access (#3712) (#477) Signed-off-by: Mamoru Sobue --- .../src/scene_module/intersection/scene_intersection.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index d005243566ee7..96a307c6195b3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -324,7 +324,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_line_idx = occlusion_first_stop_line_idx; // insert creep velocity [default_stop_line, occlusion_stop_line) insert_creep_during_occlusion = - std::make_pair(default_stop_line_idx_opt.value(), occlusion_peeking_line_idx_opt.value()); + default_stop_line_idx_opt && occlusion_peeking_line_idx_opt + ? std::make_optional>( + default_stop_line_idx_opt.value(), occlusion_peeking_line_idx_opt.value()) + : std::nullopt; occlusion_state_ = OcclusionState::BEFORE_FIRST_STOP_LINE; } } else if (occlusion_state_ != OcclusionState::CLEARED) { From beae999bcf5733101f52785c10c12290797a7e42 Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Tue, 16 May 2023 11:11:53 +0900 Subject: [PATCH 08/13] =?UTF-8?q?fix(behavior=5Fpath=5Fplanner):=20handle?= =?UTF-8?q?=20overlap=20lane=20in=20isEgoOutOfRoute()=20=E2=80=A6=20(#481)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit fix(behavior_path_planner): handle overlap lane in isEgoOutOfRoute() (#3715) Signed-off-by: Fumiya Watanabe Co-authored-by: Fumiya Watanabe --- planning/behavior_path_planner/src/utils/utils.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 9967ff835672a..a1db0d7d3e899 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1018,7 +1018,10 @@ bool isEgoOutOfRoute( } // If ego vehicle is over goal on goal lane, return true - if (lanelet::utils::isInLanelet(self_pose, goal_lane)) { + lanelet::ConstLanelet closest_lane; + const double yaw_threshold = tier4_autoware_utils::deg2rad(90); + if (lanelet::utils::query::getClosestLaneletWithConstrains( + {goal_lane}, self_pose, &closest_lane, 0.0, yaw_threshold)) { constexpr double buffer = 1.0; const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose); const auto goal_arc_coord = From 19b771741cfc30da0935ea6fbf19472c14bd6d8c Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 16 May 2023 11:12:16 +0900 Subject: [PATCH 09/13] fix(behavior_path_planner): init idling module (#3709) (#479) Signed-off-by: satoshi-ota Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../scene_module/scene_module_manager_interface.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index 412bab01d6b9c..d596fde7d8b9c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -74,6 +74,7 @@ class SceneModuleManagerInterface SceneModulePtr getNewModule() { if (idling_module_ != nullptr) { + idling_module_->onEntry(); return idling_module_; } From bed1771ac8b0385820c4e1d734d3ba11e6ef0963 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 16 May 2023 12:00:19 +0900 Subject: [PATCH 10/13] fix(avoidance): clear waiting approval only when all target module are unavoidable (#3662) (#482) Signed-off-by: satoshi-ota Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../scene_module/avoidance/avoidance_module.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 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 aa64e2741e52d..53b18545c3c79 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 @@ -2877,24 +2877,35 @@ CandidateOutput AvoidanceModule::planCandidate() const BehaviorModuleOutput AvoidanceModule::planWaitingApproval() { + const auto & data = avoidance_data_; + // we can execute the plan() since it handles the approval appropriately. BehaviorModuleOutput out = plan(); + #ifndef USE_OLD_ARCHITECTURE if (path_shifter_.getShiftLines().empty()) { out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } #endif + + const auto all_unavoidable = std::all_of( + data.target_objects.begin(), data.target_objects.end(), + [](const auto & o) { return !o.is_avoidable; }); + const auto candidate = planCandidate(); - constexpr double threshold_to_update_status = -1.0e-03; - if (candidate.start_distance_to_path_change > threshold_to_update_status) { + if (!avoidance_data_.safe_new_sl.empty()) { updateCandidateRTCStatus(candidate); waitApproval(); + } else if (all_unavoidable) { + waitApproval(); } else { clearWaitingApproval(); removeCandidateRTCStatus(); } + path_candidate_ = std::make_shared(candidate.path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; + return out; } @@ -3142,6 +3153,7 @@ void AvoidanceModule::updateData() void AvoidanceModule::processOnEntry() { initVariables(); + waitApproval(); } void AvoidanceModule::processOnExit() From d1c946a1f9ce1bcfb11096948b4d26256ce55d42 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 16 May 2023 13:40:11 +0900 Subject: [PATCH 11/13] test(planning_test_utils): add ASSART_NO_THROW_WITH_ERROR_MSG macro (#3695) * test(planning_test_utils): add ASSART_NO_THROW_WITH_ERROR_MSG macro Signed-off-by: Takamasa Horibe * update for behavior_velocity_planner Signed-off-by: Takamasa Horibe * fix for exception error msg Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../test_behavior_path_planner_node_interface.cpp | 8 ++++---- ...t_behavior_velocity_planner_node_interface.cpp | 8 ++++---- .../test_freespace_planner_node_interface.cpp | 8 ++++---- ..._obstacle_avoidance_planner_node_interface.cpp | 4 ++-- ...est_obstacle_cruise_planner_node_interface.cpp | 8 ++++---- .../test_obstacle_stop_planner_node_interface.cpp | 6 +++--- .../planning_interface_test_manager.hpp | 15 +++++++++++++++ .../planning_interface_test_manager_utils.hpp | 9 ++++++++- 8 files changed, 44 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index 28eac75a17e0a..a24086a3df8d8 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -98,11 +98,11 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - ASSERT_NO_THROW(test_manager->testWithAbnormalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); rclcpp::shutdown(); } @@ -115,12 +115,12 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); rclcpp::shutdown(); } diff --git a/planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp index 6d251aee70677..00e670f0b0ea0 100644 --- a/planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp @@ -128,11 +128,11 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) publishMandatoryTopics(test_manager, test_target_node); // test with nominal path_with_lane_id - ASSERT_NO_THROW(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty path_with_lane_id - ASSERT_NO_THROW(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); rclcpp::shutdown(); } @@ -145,12 +145,12 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); // make sure behavior_path_planner is running EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); rclcpp::shutdown(); } diff --git a/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp index c3828c3268201..8c6336b2f45d6 100644 --- a/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -68,11 +68,11 @@ TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInpu publishMandatoryTopics(test_manager, test_target_node); // test with normal route - ASSERT_NO_THROW(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with empty route - ASSERT_NO_THROW(test_manager->testWithAbnormalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); rclcpp::shutdown(); } @@ -85,10 +85,10 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); // test for normal route - ASSERT_NO_THROW(test_manager->testWithBehaviorNominalRoute(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - ASSERT_NO_THROW(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); rclcpp::shutdown(); } diff --git a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp b/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp index 5ca4e2d29b912..75b61f80622d1 100644 --- a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp +++ b/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp @@ -54,12 +54,12 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) test_manager->setPathInputTopicName("obstacle_avoidance_planner/input/path"); // test with normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalPath(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test with trajectory with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testWithAbnormalPath(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPath(test_target_node)); rclcpp::shutdown(); } diff --git a/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp b/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp index d15ea874a3a74..5be04ab801e14 100644 --- a/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp +++ b/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp @@ -75,11 +75,11 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node)); rclcpp::shutdown(); } @@ -93,11 +93,11 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); rclcpp::shutdown(); } diff --git a/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp b/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp index e09f4b4ffe557..95afdd7f2ea3b 100644 --- a/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp +++ b/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp @@ -78,7 +78,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); @@ -97,11 +97,11 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); EXPECT_GE(test_manager->getReceivedTopicNum(), 1); // test for trajectory with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); rclcpp::shutdown(); } diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index 9280aad2cce7a..5edc7b00f94a4 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -15,6 +15,21 @@ #ifndef PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_HPP_ #define PLANNING_INTERFACE_TEST_MANAGER__PLANNING_INTERFACE_TEST_MANAGER_HPP_ +// since ASSERT_NO_THROW in gtest masks the exception message, redefine it. +#define ASSERT_NO_THROW_WITH_ERROR_MSG(statement) \ + try { \ + statement; \ + SUCCEED(); \ + } catch (const std::exception & e) { \ + FAIL() << "Expected: " << #statement \ + << " doesn't throw an exception.\nActual: it throws. Error message: " << e.what() \ + << std::endl; \ + } catch (...) { \ + FAIL() << "Expected: " << #statement \ + << " doesn't throw an exception.\nActual: it throws. Error message is not available." \ + << std::endl; \ + } + #include #include #include diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp index 685ca5f69d404..3ed3ffb7a5eb5 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp @@ -41,6 +41,7 @@ #include +#include #include #include #include @@ -357,7 +358,13 @@ void publishToTargetNode( typename rclcpp::Publisher::SharedPtr publisher, T data, const int repeat_count = 3) { if (topic_name.empty()) { - throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); + int status; + char * demangled_name = abi::__cxa_demangle(typeid(data).name(), nullptr, nullptr, &status); + if (status == 0) { + throw std::runtime_error(std::string("Topic name for ") + demangled_name + " is empty"); + } else { + throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); + } } test_utils::setPublisher(test_node, topic_name, publisher); From 689bec5596132b0dead8c49387d9e1cf2f8374e0 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 16 May 2023 14:01:39 +0900 Subject: [PATCH 12/13] fix(geometric_parallel_parking): add check for empty path Signed-off-by: Takamasa Horibe --- .../geometric_parallel_parking/geometric_parallel_parking.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 03e79ef5e57ff..fb99d04f89e91 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -352,7 +352,9 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start auto path = planner_data_->route_handler->getCenterLinePath( current_lanes, current_arc_position.length, start_arc_position.length, true); path.header = planner_data_->route_handler->getRouteHeader(); - path.points.back().point.longitudinal_velocity_mps = 0; + if (!path.points.empty()) { + path.points.back().point.longitudinal_velocity_mps = 0; + } return path; } From 786ee533c194ad86ca1f45ce719b9955a717c8a6 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 11 May 2023 18:32:20 +0900 Subject: [PATCH 13/13] fix(behavior_path_planner): fix executor association with node (#3661) fix executor association with node Signed-off-by: kyoichi-sugahara --- .../planning_interface_test_manager_utils.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp index 3ed3ffb7a5eb5..15ff71b1e9f5f 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp @@ -344,10 +344,11 @@ void spinSomeNodes( rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, const int repeat_count = 1) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node); + executor.add_node(target_node); for (int i = 0; i < repeat_count; i++) { - rclcpp::spin_some(test_node); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(target_node); + executor.spin_some(std::chrono::milliseconds(100)); rclcpp::sleep_for(std::chrono::milliseconds(100)); } }