diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 451ade3771f61..635fcad9d3f55 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -190,7 +190,7 @@ planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp -planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp +planning/autoware_planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 97bf6414189ab..1d01732454daf 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -32,7 +32,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index cf96cd39043ce..4f6616823e076 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -59,6 +59,7 @@ autoware_behavior_velocity_planner autoware_path_optimizer + autoware_planning_validator autoware_remaining_distance_time_calculator autoware_velocity_smoother behavior_path_planner @@ -72,7 +73,6 @@ obstacle_stop_planner planning_evaluator planning_topic_converter - planning_validator scenario_selector surround_obstacle_checker diff --git a/planning/autoware_planning_test_manager/README.md b/planning/autoware_planning_test_manager/README.md index fe5484ca498ee..97eb2fd8d94fd 100644 --- a/planning/autoware_planning_test_manager/README.md +++ b/planning/autoware_planning_test_manager/README.md @@ -41,7 +41,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", - planning_validator_dir + "/config/planning_validator.param.yaml"}); + autoware_planning_validator_dir + "/config/planning_validator.param.yaml"}); // instantiate the TargetNode with node_options auto test_target_node = std::make_shared(node_options); @@ -70,18 +70,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) ## Implemented tests -| Node | Test name | exceptional input | output | Exceptional input pattern | -| ------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | -| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| path_optimizer | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | -| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | -| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | -| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | +| Node | Test name | exceptional input | output | Exceptional input pattern | +| --------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | +| autoware_planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| path_optimizer | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | +| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | +| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | +| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | ## Important Notes diff --git a/planning/planning_validator/CMakeLists.txt b/planning/planning_validator/CMakeLists.txt index 455dded2d7e32..da780cfdebdcf 100644 --- a/planning/planning_validator/CMakeLists.txt +++ b/planning/planning_validator/CMakeLists.txt @@ -1,22 +1,22 @@ cmake_minimum_required(VERSION 3.22) -project(planning_validator) +project(autoware_planning_validator) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(planning_validator_helpers SHARED +ament_auto_add_library(autoware_planning_validator_helpers SHARED src/utils.cpp src/debug_marker.cpp ) # planning validator -ament_auto_add_library(planning_validator_component SHARED - include/planning_validator/planning_validator.hpp +ament_auto_add_library(autoware_planning_validator_component SHARED + include/autoware_planning_validator/planning_validator.hpp src/planning_validator.cpp ) -target_link_libraries(planning_validator_component planning_validator_helpers) -rclcpp_components_register_node(planning_validator_component - PLUGIN "planning_validator::PlanningValidator" +target_link_libraries(autoware_planning_validator_component autoware_planning_validator_helpers) +rclcpp_components_register_node(autoware_planning_validator_component + PLUGIN "autoware::planning_validator::PlanningValidator" EXECUTABLE planning_validator_node ) @@ -25,7 +25,7 @@ ament_auto_add_library(invalid_trajectory_publisher_node SHARED src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp ) rclcpp_components_register_node(invalid_trajectory_publisher_node - PLUGIN "planning_validator::InvalidTrajectoryPublisherNode" + PLUGIN "autoware::planning_validator::InvalidTrajectoryPublisherNode" EXECUTABLE invalid_trajectory_publisher ) @@ -37,29 +37,29 @@ rosidl_generate_interfaces( # to use a message defined in the same package if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(planning_validator_component + rosidl_target_interfaces(autoware_planning_validator_component ${PROJECT_NAME} "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(planning_validator_component "${cpp_typesupport_target}") + target_link_libraries(autoware_planning_validator_component "${cpp_typesupport_target}") endif() if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_planning_validator + ament_add_ros_isolated_gtest(test_autoware_planning_validator test/src/test_main.cpp test/src/test_planning_validator_functions.cpp test/src/test_planning_validator_helper.cpp test/src/test_planning_validator_pubsub.cpp - test/src/test_${PROJECT_NAME}_node_interface.cpp + test/src/test_planning_validator_node_interface.cpp ) - ament_target_dependencies(test_planning_validator + ament_target_dependencies(test_autoware_planning_validator rclcpp autoware_planning_msgs ) - target_link_libraries(test_planning_validator - planning_validator_component + target_link_libraries(test_autoware_planning_validator + autoware_planning_validator_component ) endif() diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md index 9d70e7f78a5bb..896797400544a 100644 --- a/planning/planning_validator/README.md +++ b/planning/planning_validator/README.md @@ -1,8 +1,8 @@ # Planning Validator -The `planning_validator` is a module that checks the validity of a trajectory before it is published. The status of the validation can be viewed in the `/diagnostics` and `/validation_status` topics. When an invalid trajectory is detected, the `planning_validator` will process the trajectory following the selected option: "0. publish the trajectory as it is", "1. stop publishing the trajectory", "2. publish the last validated trajectory". +The `autoware_planning_validator` is a module that checks the validity of a trajectory before it is published. The status of the validation can be viewed in the `/diagnostics` and `/validation_status` topics. When an invalid trajectory is detected, the `autoware_planning_validator` will process the trajectory following the selected option: "0. publish the trajectory as it is", "1. stop publishing the trajectory", "2. publish the last validated trajectory". -![planning_validator](./image/planning_validator.drawio.svg) +![autoware_planning_validator](./image/autoware_planning_validator.drawio.svg) ## Supported features @@ -29,7 +29,7 @@ The following features are to be implemented. ### Inputs -The `planning_validator` takes in the following inputs: +The `autoware_planning_validator` takes in the following inputs: | Name | Type | Description | | -------------------- | --------------------------------- | ---------------------------------------------- | @@ -48,7 +48,7 @@ It outputs the following: ## Parameters -The following parameters can be set for the `planning_validator`: +The following parameters can be set for the `autoware_planning_validator`: ### System parameters diff --git a/planning/planning_validator/config/planning_validator_plotjugler_config.xml b/planning/planning_validator/config/planning_validator_plotjugler_config.xml index d6afc0b017d32..04d5d4d8a0e38 100644 --- a/planning/planning_validator/config/planning_validator_plotjugler_config.xml +++ b/planning/planning_validator/config/planning_validator_plotjugler_config.xml @@ -9,21 +9,21 @@ - + - + - + @@ -32,21 +32,21 @@ - + - + - + @@ -55,21 +55,21 @@ - + - + - + @@ -78,7 +78,7 @@ - + @@ -86,23 +86,23 @@ - - - - - + + + + + - - - - - - + + + + + + diff --git a/planning/planning_validator/image/planning_validator.drawio.svg b/planning/planning_validator/image/planning_validator.drawio.svg index 070c1a6d68b81..0a2267cba159e 100644 --- a/planning/planning_validator/image/planning_validator.drawio.svg +++ b/planning/planning_validator/image/planning_validator.drawio.svg @@ -38,12 +38,12 @@ >
- planning_validator + autoware_planning_validator
- planning_validator + autoware_planning_validator diff --git a/planning/planning_validator/include/planning_validator/debug_marker.hpp b/planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp similarity index 91% rename from planning/planning_validator/include/planning_validator/debug_marker.hpp rename to planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp index 4671a0b0d7297..955ba5bbf64ab 100644 --- a/planning/planning_validator/include/planning_validator/debug_marker.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ -#define PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ #include @@ -57,4 +57,4 @@ class PlanningValidatorDebugMarkerPublisher } }; -#endif // PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp similarity index 91% rename from planning/planning_validator/include/planning_validator/planning_validator.hpp rename to planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp index fed6e161c4d90..113c70fecb62c 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ -#define PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ -#include "planning_validator/debug_marker.hpp" -#include "planning_validator/msg/planning_validator_status.hpp" +#include "autoware_planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/msg/planning_validator_status.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -33,14 +33,14 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_validator::msg::PlanningValidatorStatus; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; -using planning_validator::msg::PlanningValidatorStatus; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; @@ -141,6 +141,6 @@ class PlanningValidator : public rclcpp::Node StopWatch stop_watch_; }; -} // namespace planning_validator +} // namespace autoware::planning_validator -#endif // PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/autoware_planning_validator/utils.hpp similarity index 90% rename from planning/planning_validator/include/planning_validator/utils.hpp rename to planning/planning_validator/include/autoware_planning_validator/utils.hpp index 11eaf4f37f400..1c943f5a60e08 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__UTILS_HPP_ -#define PLANNING_VALIDATOR__UTILS_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -65,6 +65,6 @@ bool checkFinite(const TrajectoryPoint & point); void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal); -} // namespace planning_validator +} // namespace autoware::planning_validator -#endif // PLANNING_VALIDATOR__UTILS_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ diff --git a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml index 3f73e0f462cc2..a1cdc7c7ecc55 100644 --- a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml +++ b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/planning/planning_validator/launch/planning_validator.launch.xml b/planning/planning_validator/launch/planning_validator.launch.xml index 81573adb4dc24..fa1475a0787e4 100644 --- a/planning/planning_validator/launch/planning_validator.launch.xml +++ b/planning/planning_validator/launch/planning_validator.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 5a413f27dc7a3..7fe57e68928d9 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -1,9 +1,9 @@ - planning_validator + autoware_planning_validator 0.1.0 - ros node for planning_validator + ros node for autoware_planning_validator Takamasa Horibe Kosuke Takeuchi Apache License 2.0 diff --git a/planning/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp index 82117d199c36e..413611a72b5ab 100644 --- a/planning/planning_validator/src/debug_marker.cpp +++ b/planning/planning_validator/src/debug_marker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/debug_marker.hpp" #include #include @@ -90,7 +90,7 @@ void PlanningValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs: { const auto now = node_->get_clock()->now(); const auto stop_wall_marker = - motion_utils::createStopVirtualWallMarker(pose, "planning_validator", now, 0); + motion_utils::createStopVirtualWallMarker(pose, "autoware_planning_validator", now, 0); tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp index 6cca99cd9f14c..645d6c92e3698 100644 --- a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp @@ -18,7 +18,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { InvalidTrajectoryPublisherNode::InvalidTrajectoryPublisherNode( const rclcpp::NodeOptions & node_options) @@ -72,7 +72,7 @@ void InvalidTrajectoryPublisherNode::onCurrentTrajectory(const Trajectory::Const traj_sub_.reset(); } -} // namespace planning_validator +} // namespace autoware::planning_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_validator::InvalidTrajectoryPublisherNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_validator::InvalidTrajectoryPublisherNode) diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp index 1d1e47ad10407..0025d759217bf 100644 --- a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp @@ -21,7 +21,7 @@ #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -41,6 +41,6 @@ class InvalidTrajectoryPublisherNode : public rclcpp::Node Trajectory::ConstSharedPtr current_trajectory_ = nullptr; }; -} // namespace planning_validator +} // namespace autoware::planning_validator #endif // INVALID_TRAJECTORY_PUBLISHER__INVALID_TRAJECTORY_PUBLISHER_HPP_ diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index d14b1d9ff37d2..aac50e24c5d5c 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" -#include "planning_validator/utils.hpp" +#include "autoware_planning_validator/utils.hpp" #include #include @@ -23,7 +23,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using diagnostic_msgs::msg::DiagnosticStatus; @@ -584,7 +584,7 @@ void PlanningValidator::displayStatus() warn(s.is_valid_forward_trajectory_length, "planning trajectory forward length is not enough!!"); } -} // namespace planning_validator +} // namespace autoware::planning_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_validator::PlanningValidator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_validator::PlanningValidator) diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index aada0bea2da9b..4710d3ae84ea3 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/utils.hpp" +#include "autoware_planning_validator/utils.hpp" #include #include @@ -22,7 +22,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using tier4_autoware_utils::calcCurvature; using tier4_autoware_utils::calcDistance2d; @@ -324,4 +324,4 @@ void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal) pose.position.y += std::sin(yaw) * longitudinal; } -} // namespace planning_validator +} // namespace autoware::planning_validator diff --git a/planning/planning_validator/test/src/test_planning_validator_functions.cpp b/planning/planning_validator/test/src/test_planning_validator_functions.cpp index e406b6c5d7da2..41ae41a80bdc5 100644 --- a/planning/planning_validator/test/src/test_planning_validator_functions.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_functions.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/debug_marker.hpp" -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" @@ -21,8 +21,8 @@ #include +using autoware::planning_validator::PlanningValidator; using autoware_planning_msgs::msg::Trajectory; -using planning_validator::PlanningValidator; TEST(PlanningValidatorTestSuite, checkValidFiniteValueFunction) { diff --git a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp index 2b2a32bf54618..78f57e0fe104d 100644 --- a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include #include @@ -22,8 +22,8 @@ #include +using autoware::planning_validator::PlanningValidator; using planning_test_utils::PlanningInterfaceTestManager; -using planning_validator::PlanningValidator; std::shared_ptr generateTestManager() { @@ -46,7 +46,7 @@ std::shared_ptr generateNode() const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto planning_validator_dir = - ament_index_cpp::get_package_share_directory("planning_validator"); + ament_index_cpp::get_package_share_directory("autoware_planning_validator"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp index 2329a3b9bb6c8..2217ee12c322f 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" @@ -29,11 +29,11 @@ * This test checks the diagnostics message published from the planning_validator node */ +using autoware::planning_validator::PlanningValidator; using autoware_planning_msgs::msg::Trajectory; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; -using planning_validator::PlanningValidator; constexpr double epsilon = 0.001; constexpr double scale_margin = 1.1; @@ -101,7 +101,8 @@ bool hasError(const std::vector & diags, const throw std::runtime_error(name + " is not contained in the diagnostic message."); } -std::pair, std::shared_ptr> +std::pair< + std::shared_ptr, std::shared_ptr> prepareTest(const Trajectory & trajectory, const Odometry & ego_odom) { auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); @@ -181,7 +182,7 @@ TEST(PlanningValidator, DiagCheckForTooLongIntervalTrajectory) TEST(PlanningValidator, DiagCheckSize) { - const auto diag_name = "planning_validator: trajectory_validation_size"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_size"; const auto odom = generateDefaultOdometry(); runWithBadTrajectory(generateTrajectory(1.0, 1.0, 0.0, 0), odom, diag_name); runWithBadTrajectory(generateTrajectory(1.0, 1.0, 0.0, 1), odom, diag_name); @@ -191,7 +192,7 @@ TEST(PlanningValidator, DiagCheckSize) TEST(PlanningValidator, DiagCheckInterval) { - const auto diag_name = "planning_validator: trajectory_validation_interval"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_interval"; const auto odom = generateDefaultOdometry(); // Larger interval than threshold -> must be NG @@ -215,7 +216,7 @@ TEST(PlanningValidator, DiagCheckInterval) TEST(PlanningValidator, DiagCheckRelativeAngle) { - const auto diag_name = "planning_validator: trajectory_validation_relative_angle"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_relative_angle"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -245,7 +246,7 @@ TEST(PlanningValidator, DiagCheckRelativeAngle) TEST(PlanningValidator, DiagCheckCurvature) { - const auto diag_name = "planning_validator: trajectory_validation_curvature"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_curvature"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -278,7 +279,7 @@ TEST(PlanningValidator, DiagCheckCurvature) TEST(PlanningValidator, DiagCheckLateralAcceleration) { - const auto diag_name = "planning_validator: trajectory_validation_lateral_acceleration"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_lateral_acceleration"; constexpr double speed = 10.0; // Note: lateral_acceleration is speed^2 * curvature; @@ -302,7 +303,7 @@ TEST(PlanningValidator, DiagCheckLateralAcceleration) TEST(PlanningValidator, DiagCheckLongitudinalMaxAcc) { - const auto diag_name = "planning_validator: trajectory_validation_acceleration"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_acceleration"; constexpr double speed = 1.0; // Larger acceleration than threshold -> must be NG @@ -324,7 +325,7 @@ TEST(PlanningValidator, DiagCheckLongitudinalMaxAcc) TEST(PlanningValidator, DiagCheckLongitudinalMinAcc) { - const auto diag_name = "planning_validator: trajectory_validation_deceleration"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_deceleration"; constexpr double speed = 20.0; const auto test = [&](const auto acceleration, const bool expect_ok) { @@ -345,7 +346,7 @@ TEST(PlanningValidator, DiagCheckLongitudinalMinAcc) TEST(PlanningValidator, DiagCheckSteering) { - const auto diag_name = "planning_validator: trajectory_validation_steering"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_steering"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -369,7 +370,7 @@ TEST(PlanningValidator, DiagCheckSteering) TEST(PlanningValidator, DiagCheckSteeringRate) { - const auto diag_name = "planning_validator: trajectory_validation_steering_rate"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_steering_rate"; // TODO(Horibe): interval must be larger than min_interval used in planning_validator.cpp constexpr auto interval = 1.1; @@ -393,7 +394,7 @@ TEST(PlanningValidator, DiagCheckSteeringRate) TEST(PlanningValidator, DiagCheckVelocityDeviation) { - const auto diag_name = "planning_validator: trajectory_validation_velocity_deviation"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_velocity_deviation"; const auto test = [&](const auto trajectory_speed, const auto ego_speed, const bool expect_ok) { const auto trajectory = generateTrajectory(1.0, trajectory_speed, 0.0, 10); const auto ego_odom = generateDefaultOdometry(0.0, 0.0, ego_speed); @@ -411,7 +412,7 @@ TEST(PlanningValidator, DiagCheckVelocityDeviation) TEST(PlanningValidator, DiagCheckDistanceDeviation) { - const auto diag_name = "planning_validator: trajectory_validation_distance_deviation"; + const auto diag_name = "autoware_planning_validator: trajectory_validation_distance_deviation"; const auto test = [&](const auto ego_x, const auto ego_y, const bool expect_ok) { const auto trajectory = generateTrajectory(1.0, 3.0, 0.0, 10); const auto last_p = trajectory.points.back().pose.position; @@ -438,7 +439,7 @@ TEST(PlanningValidator, DiagCheckDistanceDeviation) TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation) { const auto diag_name = - "planning_validator: trajectory_validation_longitudinal_distance_deviation"; + "autoware_planning_validator: trajectory_validation_longitudinal_distance_deviation"; const auto trajectory = generateTrajectory(1.0, 3.0, 0.0, 10); const auto test = [&](const auto ego_x, const auto ego_y, const bool expect_ok) { const auto ego_odom = generateDefaultOdometry(ego_x, ego_y, 0.0); @@ -468,7 +469,8 @@ TEST(PlanningValidator, DiagCheckLongitudinalDistanceDeviation) TEST(PlanningValidator, DiagCheckForwardTrajectoryLength) { - const auto diag_name = "planning_validator: trajectory_validation_forward_trajectory_length"; + const auto diag_name = + "autoware_planning_validator: trajectory_validation_forward_trajectory_length"; constexpr auto trajectory_v = 10.0; constexpr size_t trajectory_size = 10; constexpr auto ego_v = 10.0;