diff --git a/planning/autoware_trajectory_safety_filter/config/trajectory_safety_filter.param.yaml b/planning/autoware_trajectory_safety_filter/config/trajectory_safety_filter.param.yaml deleted file mode 100644 index 93f21ca3b651a..0000000000000 --- a/planning/autoware_trajectory_safety_filter/config/trajectory_safety_filter.param.yaml +++ /dev/null @@ -1,38 +0,0 @@ -/**: - ros__parameters: - filter_names: - [ - "autoware::trajectory_safety_filter::plugin::OutOfLaneFilter", - "autoware::trajectory_safety_filter::plugin::CollisionFilter", - "autoware::trajectory_safety_filter::plugin::CollisionCheckFilter", - "autoware::trajectory_safety_filter::plugin::UncrossableBoundaryDepartureFilter", - "autoware::trajectory_safety_filter::plugin::VehicleConstraintFilter", - ] - # available filters: - # "autoware::trajectory_safety_filter::plugin::OutOfLaneFilter" - # "autoware::trajectory_safety_filter::plugin::CollisionFilter" - # "autoware::trajectory_safety_filter::plugin::CollisionCheckFilter" - # "autoware::trajectory_safety_filter::plugin::UncrossableBoundaryDepartureFilter" - # "autoware::trajectory_safety_filter::plugin::VehicleConstraintFilter" - - out_of_lane: - time: 2.0 - min_value: 0.0 - - collision: - time: 3.0 - min_value: 2.0 - - collision_check: - pet_collision: - ego_braking_delay: 0.4 - ego_assumed_acceleration: -5.0 - collision_time_threshold: 0.0 - - - vehicle_constraint: - max_speed: 16.7 # m/s - max_acceleration: 5.0 # m/s^2 - max_deceleration: 5.0 # m/s^2 (positive but represents deceleration) - max_steering_angle: 0.8 # rad - max_steering_rate: 0.3 # rad/s diff --git a/planning/autoware_trajectory_safety_filter/plugins.xml b/planning/autoware_trajectory_safety_filter/plugins.xml deleted file mode 100644 index a9197d4e71735..0000000000000 --- a/planning/autoware_trajectory_safety_filter/plugins.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - - Filters trajectories that go out of lane boundaries - - - - - Filters trajectories based on Time To Collision (TTC) with predicted objects - - - - - Filters trajectories that cross uncrossable boundaries - - - - - Filters trajectories that violate vehicle constraints such as maximum speed, acceleration, and steering angle - - - - - Filters trajectories based on collision checks with predicted objects - - - diff --git a/planning/autoware_trajectory_safety_filter/test/dummy_plugin/dummy_plugins.xml b/planning/autoware_trajectory_safety_filter/test/dummy_plugin/dummy_plugins.xml deleted file mode 100644 index 433397d5eed61..0000000000000 --- a/planning/autoware_trajectory_safety_filter/test/dummy_plugin/dummy_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - A predictable dummy filter for testing the main node - - diff --git a/planning/autoware_trajectory_safety_filter/CMakeLists.txt b/planning/autoware_trajectory_validator/CMakeLists.txt similarity index 64% rename from planning/autoware_trajectory_safety_filter/CMakeLists.txt rename to planning/autoware_trajectory_validator/CMakeLists.txt index 640b41cca92a9..943b98b2293b0 100644 --- a/planning/autoware_trajectory_safety_filter/CMakeLists.txt +++ b/planning/autoware_trajectory_validator/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.14) -project(autoware_trajectory_safety_filter) +project(autoware_trajectory_validator) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(autoware_trajectory_safety_filter plugins.xml) +pluginlib_export_plugin_description_file(autoware_trajectory_validator plugins.xml) generate_parameter_library(${PROJECT_NAME}_param param/parameter_struct.yaml @@ -14,7 +14,7 @@ include_directories( ) ament_auto_add_library(${PROJECT_NAME} SHARED - src/trajectory_safety_filter_node.cpp + src/trajectory_validator_node.cpp ) target_link_libraries(${PROJECT_NAME} @@ -26,19 +26,19 @@ target_compile_options(${PROJECT_NAME} PUBLIC ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "autoware::trajectory_safety_filter::TrajectorySafetyFilter" + PLUGIN "autoware::trajectory_validator::TrajectoryValidator" EXECUTABLE ${PROJECT_NAME}_node ) -ament_auto_add_library(autoware_trajectory_safety_filter_plugins SHARED - src/filters/out_of_lane_filter.cpp - src/filters/collision_filter.cpp - src/filters/uncrossable_boundary_departure_filter.cpp - src/filters/vehicle_constraint_filter.cpp - src/filters/collision_check_filter.cpp +ament_auto_add_library(autoware_trajectory_validator_plugins SHARED + src/filters/safety/out_of_lane_filter.cpp + src/filters/safety/collision_filter.cpp + src/filters/safety/uncrossable_boundary_departure_filter.cpp + src/filters/safety/vehicle_constraint_filter.cpp + src/filters/safety/collision_check_filter.cpp ) -target_link_libraries(autoware_trajectory_safety_filter_plugins +target_link_libraries(autoware_trajectory_validator_plugins ${PROJECT_NAME} ) @@ -54,7 +54,7 @@ if(BUILD_TESTING) test/plugins/test_vehicle_constraint_filter.cpp ) target_link_libraries(test_vehicle_constraint_filter - autoware_trajectory_safety_filter_plugins + autoware_trajectory_validator_plugins ) ament_add_ros_isolated_gtest(test_constant_curvature @@ -80,31 +80,31 @@ if(BUILD_TESTING) # ========================================== # 2. Dummy Plugin (Test Utility) # ========================================== - pluginlib_export_plugin_description_file(autoware_trajectory_safety_filter + pluginlib_export_plugin_description_file(autoware_trajectory_validator test/dummy_plugin/dummy_plugins.xml ) - ament_auto_add_library(dummy_safety_filter_plugin SHARED - test/dummy_plugin/dummy_safety_filter_plugin.cpp + ament_auto_add_library(dummy_validator_plugin SHARED + test/dummy_plugin/dummy_validator_plugin.cpp ) - target_link_libraries(dummy_safety_filter_plugin + target_link_libraries(dummy_validator_plugin ${PROJECT_NAME} ) # ========================================== # 3. Node Integration/Unit Tests # ========================================== - ament_add_ros_isolated_gtest(test_trajectory_safety_filter_node - test/node/test_trajectory_safety_filter_node.cpp + ament_add_ros_isolated_gtest(test_trajectory_validator_node + test/node/test_trajectory_validator_node.cpp ) - target_link_libraries(test_trajectory_safety_filter_node + target_link_libraries(test_trajectory_validator_node ${PROJECT_NAME} - dummy_safety_filter_plugin + dummy_validator_plugin ) - ament_target_dependencies(test_trajectory_safety_filter_node + ament_target_dependencies(test_trajectory_validator_node autoware_test_utils ) endif() diff --git a/planning/autoware_trajectory_safety_filter/README.md b/planning/autoware_trajectory_validator/README.md similarity index 100% rename from planning/autoware_trajectory_safety_filter/README.md rename to planning/autoware_trajectory_validator/README.md diff --git a/planning/autoware_trajectory_validator/config/trajectory_validator.param.yaml b/planning/autoware_trajectory_validator/config/trajectory_validator.param.yaml new file mode 100644 index 0000000000000..3aa30ee67a935 --- /dev/null +++ b/planning/autoware_trajectory_validator/config/trajectory_validator.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + filter_names: + [ + "autoware::trajectory_validator::plugin::OutOfLaneFilter", + "autoware::trajectory_validator::plugin::CollisionFilter", + "autoware::trajectory_validator::plugin::CollisionCheckFilter", + "autoware::trajectory_validator::plugin::UncrossableBoundaryDepartureFilter", + "autoware::trajectory_validator::plugin::VehicleConstraintFilter", + ] + # available filters: + # "autoware::trajectory_validator::plugin::OutOfLaneFilter" + # "autoware::trajectory_validator::plugin::CollisionFilter" + # "autoware::trajectory_validator::plugin::CollisionCheckFilter" + # "autoware::trajectory_validator::plugin::UncrossableBoundaryDepartureFilter" + # "autoware::trajectory_validator::plugin::VehicleConstraintFilter" + + out_of_lane: + time: 2.0 + min_value: 0.0 + + collision: + time: 3.0 + min_value: 2.0 + + collision_check: + pet_collision: + ego_braking_delay: 0.4 + ego_assumed_acceleration: -5.0 + collision_time_threshold: 0.0 + + + vehicle_constraint: + max_speed: 16.7 # m/s + max_acceleration: 5.0 # m/s^2 + max_deceleration: 5.0 # m/s^2 (positive but represents deceleration) + max_steering_angle: 0.8 # rad + max_steering_rate: 0.3 # rad/s diff --git a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filter_context.hpp b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filter_context.hpp similarity index 82% rename from planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filter_context.hpp rename to planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filter_context.hpp index 2cb3548ce32e8..f6e8d088fe3b7 100644 --- a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filter_context.hpp +++ b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filter_context.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTER_CONTEXT_HPP_ -#define AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTER_CONTEXT_HPP_ +#ifndef AUTOWARE__TRAJECTORY_VALIDATOR__FILTER_CONTEXT_HPP_ +#define AUTOWARE__TRAJECTORY_VALIDATOR__FILTER_CONTEXT_HPP_ #include #include @@ -29,7 +29,7 @@ #include #include -namespace autoware::trajectory_safety_filter +namespace autoware::trajectory_validator { // Base context that all filters can access @@ -40,6 +40,6 @@ struct FilterContext std::shared_ptr lanelet_map; autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; }; -} // namespace autoware::trajectory_safety_filter +} // namespace autoware::trajectory_validator -#endif // AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTER_CONTEXT_HPP_ +#endif // AUTOWARE__TRAJECTORY_VALIDATOR__FILTER_CONTEXT_HPP_ diff --git a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/collision_check_filter.hpp b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/collision_check_filter.hpp similarity index 89% rename from planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/collision_check_filter.hpp rename to planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/collision_check_filter.hpp index dbbbc7c904fb7..126e7829388e2 100644 --- a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/collision_check_filter.hpp +++ b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/collision_check_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__COLLISION_CHECK_FILTER_HPP_ -#define AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__COLLISION_CHECK_FILTER_HPP_ +#ifndef AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__COLLISION_CHECK_FILTER_HPP_ +#define AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__COLLISION_CHECK_FILTER_HPP_ -#include "autoware/trajectory_safety_filter/safety_filter_interface.hpp" +#include "autoware/trajectory_validator/validator_interface.hpp" #include #include @@ -33,7 +33,7 @@ #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { using autoware_utils_geometry::Box2d; using autoware_utils_geometry::MultiPoint2d; @@ -130,10 +130,10 @@ PoseTrajectory compute_pose_trajectory( return pose_trajectory; } -class CollisionCheckFilter : public SafetyFilterInterface +class CollisionCheckFilter : public ValidatorInterface { public: - CollisionCheckFilter() : SafetyFilterInterface("CollisionCheckFilter") {} + CollisionCheckFilter() : ValidatorInterface("CollisionCheckFilter") {} tl::expected is_feasible( const TrajectoryPoints & traj_points, const FilterContext & context) override; @@ -151,6 +151,6 @@ class CollisionCheckFilter : public SafetyFilterInterface } pet_collision_params_; }; -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin -#endif // AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__COLLISION_CHECK_FILTER_HPP_ +#endif // AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__COLLISION_CHECK_FILTER_HPP_ diff --git a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/collision_filter.hpp b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/collision_filter.hpp similarity index 80% rename from planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/collision_filter.hpp rename to planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/collision_filter.hpp index 23aa77db8defe..fc36855a80f28 100644 --- a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/collision_filter.hpp +++ b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/collision_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__COLLISION_FILTER_HPP_ -#define AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__COLLISION_FILTER_HPP_ +#ifndef AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__COLLISION_FILTER_HPP_ +#define AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__COLLISION_FILTER_HPP_ -#include "autoware/trajectory_safety_filter/safety_filter_interface.hpp" +#include "autoware/trajectory_validator/validator_interface.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { // Parameters for CollisionFilter @@ -36,10 +36,10 @@ struct CollisionParams double min_ttc = 2.0; // seconds - minimum acceptable time to collision }; -class CollisionFilter : public SafetyFilterInterface +class CollisionFilter : public ValidatorInterface { public: - CollisionFilter() : SafetyFilterInterface("CollisionFilter") {} + CollisionFilter() : ValidatorInterface("CollisionFilter") {} tl::expected is_feasible( const TrajectoryPoints & traj_points, const FilterContext & context) final; @@ -71,6 +71,6 @@ class CollisionFilter : public SafetyFilterInterface double time_from_start) const; }; -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin -#endif // AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__COLLISION_FILTER_HPP_ +#endif // AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__COLLISION_FILTER_HPP_ diff --git a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/out_of_lane_filter.hpp b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/out_of_lane_filter.hpp similarity index 76% rename from planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/out_of_lane_filter.hpp rename to planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/out_of_lane_filter.hpp index da0b2fb6b98a4..d3c0f4f6d9221 100644 --- a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/out_of_lane_filter.hpp +++ b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/out_of_lane_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__OUT_OF_LANE_FILTER_HPP_ -#define AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__OUT_OF_LANE_FILTER_HPP_ +#ifndef AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__OUT_OF_LANE_FILTER_HPP_ +#define AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__OUT_OF_LANE_FILTER_HPP_ -#include "autoware/trajectory_safety_filter/safety_filter_interface.hpp" +#include "autoware/trajectory_validator/validator_interface.hpp" #include #include @@ -28,7 +28,7 @@ #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { // Parameters for OutOfLaneFilter @@ -38,7 +38,7 @@ struct OutOfLaneParams double min_value = 0.0; // meters - minimum distance to lane boundary }; -class OutOfLaneFilter : public SafetyFilterInterface +class OutOfLaneFilter : public ValidatorInterface { public: OutOfLaneFilter(); @@ -55,6 +55,6 @@ class OutOfLaneFilter : public SafetyFilterInterface std::unique_ptr boundary_departure_checker_; }; -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin -#endif // AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__OUT_OF_LANE_FILTER_HPP_ +#endif // AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__OUT_OF_LANE_FILTER_HPP_ diff --git a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/uncrossable_boundary_departure_filter.hpp b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/uncrossable_boundary_departure_filter.hpp similarity index 74% rename from planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/uncrossable_boundary_departure_filter.hpp rename to planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/uncrossable_boundary_departure_filter.hpp index 2fe8e46889cf2..074536b05061f 100644 --- a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/uncrossable_boundary_departure_filter.hpp +++ b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/uncrossable_boundary_departure_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__UNCROSSABLE_BOUNDARY_DEPARTURE_FILTER_HPP_ -#define AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__UNCROSSABLE_BOUNDARY_DEPARTURE_FILTER_HPP_ +#ifndef AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__UNCROSSABLE_BOUNDARY_DEPARTURE_FILTER_HPP_ +#define AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__UNCROSSABLE_BOUNDARY_DEPARTURE_FILTER_HPP_ -#include "autoware/trajectory_safety_filter/safety_filter_interface.hpp" +#include "autoware/trajectory_validator/validator_interface.hpp" #include #include @@ -24,14 +24,12 @@ #include #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { -class UncrossableBoundaryDepartureFilter : public SafetyFilterInterface +class UncrossableBoundaryDepartureFilter : public ValidatorInterface { public: - UncrossableBoundaryDepartureFilter() : SafetyFilterInterface("UncrossableBoundaryDepartureFilter") - { - } + UncrossableBoundaryDepartureFilter() : ValidatorInterface("UncrossableBoundaryDepartureFilter") {} tl::expected is_feasible( const TrajectoryPoints & traj_points, const FilterContext & context) final; @@ -65,6 +63,6 @@ class UncrossableBoundaryDepartureFilter : public SafetyFilterInterface RCLCPP_WARN_THROTTLE(log_, *clock_, 5000, fmt, args...); } }; -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin -#endif // AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__UNCROSSABLE_BOUNDARY_DEPARTURE_FILTER_HPP_ +#endif // AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__UNCROSSABLE_BOUNDARY_DEPARTURE_FILTER_HPP_ diff --git a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/vehicle_constraint_filter.hpp b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/vehicle_constraint_filter.hpp similarity index 90% rename from planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/vehicle_constraint_filter.hpp rename to planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/vehicle_constraint_filter.hpp index 457c8868355b2..20d73042a6fee 100644 --- a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/filters/vehicle_constraint_filter.hpp +++ b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/filters/safety/vehicle_constraint_filter.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__VEHICLE_CONSTRAINT_FILTER_HPP_ -#define AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__VEHICLE_CONSTRAINT_FILTER_HPP_ +#ifndef AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__VEHICLE_CONSTRAINT_FILTER_HPP_ +#define AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__VEHICLE_CONSTRAINT_FILTER_HPP_ -#include "autoware/trajectory_safety_filter/safety_filter_interface.hpp" +#include "autoware/trajectory_validator/validator_interface.hpp" #include #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { /** * @brief VehicleConstraintFilter class - checks if the trajectory respects vehicle constraints * (e.g., max speed, max acceleration/deceleration). */ -class VehicleConstraintFilter final : public SafetyFilterInterface +class VehicleConstraintFilter final : public ValidatorInterface { public: using result_t = tl::expected; @@ -122,5 +122,5 @@ bool is_steering_angle_ok( */ bool is_steering_rate_ok( const TrajectoryPoints & traj_points, const VehicleInfo & vehicle_info, double max_steering_rate); -} // namespace autoware::trajectory_safety_filter::plugin -#endif // AUTOWARE__TRAJECTORY_SAFETY_FILTER__FILTERS__VEHICLE_CONSTRAINT_FILTER_HPP_ +} // namespace autoware::trajectory_validator::plugin +#endif // AUTOWARE__TRAJECTORY_VALIDATOR__FILTERS__SAFETY__VEHICLE_CONSTRAINT_FILTER_HPP_ diff --git a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/trajectory_safety_filter_node.hpp b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/trajectory_validator_node.hpp similarity index 81% rename from planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/trajectory_safety_filter_node.hpp rename to planning/autoware_trajectory_validator/include/autoware/trajectory_validator/trajectory_validator_node.hpp index 420fb793d041e..ce8a5407ea06d 100644 --- a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/trajectory_safety_filter_node.hpp +++ b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/trajectory_validator_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY_SAFETY_FILTER__TRAJECTORY_SAFETY_FILTER_NODE_HPP_ -#define AUTOWARE__TRAJECTORY_SAFETY_FILTER__TRAJECTORY_SAFETY_FILTER_NODE_HPP_ +#ifndef AUTOWARE__TRAJECTORY_VALIDATOR__TRAJECTORY_VALIDATOR_NODE_HPP_ +#define AUTOWARE__TRAJECTORY_VALIDATOR__TRAJECTORY_VALIDATOR_NODE_HPP_ -#include "autoware/trajectory_safety_filter/safety_filter_interface.hpp" +#include "autoware/trajectory_validator/validator_interface.hpp" #include -#include +#include #include #include #include @@ -39,7 +39,7 @@ #include #include -namespace autoware::trajectory_safety_filter +namespace autoware::trajectory_validator { using autoware_internal_planning_msgs::msg::CandidateTrajectories; using autoware_internal_planning_msgs::msg::CandidateTrajectory; @@ -50,10 +50,10 @@ using autoware_utils_diagnostics::DiagnosticsInterface; using geometry_msgs::msg::AccelWithCovarianceStamped; using nav_msgs::msg::Odometry; -class TrajectorySafetyFilter : public rclcpp::Node +class TrajectoryValidator : public rclcpp::Node { public: - explicit TrajectorySafetyFilter(const rclcpp::NodeOptions & node_options); + explicit TrajectoryValidator(const rclcpp::NodeOptions & node_options); private: void process(const CandidateTrajectories::ConstSharedPtr msg); @@ -74,7 +74,7 @@ class TrajectorySafetyFilter : public rclcpp::Node rcl_interfaces::msg::SetParametersResult on_parameter( const std::vector & parameters); - std::unique_ptr listener_; + std::unique_ptr listener_; rclcpp::Publisher::SharedPtr debug_processing_time_detail_pub_; @@ -96,14 +96,14 @@ class TrajectorySafetyFilter : public rclcpp::Node rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_res_; - pluginlib::ClassLoader plugin_loader_; + pluginlib::ClassLoader plugin_loader_; - std::vector> plugins_; + std::vector> plugins_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; - DiagnosticsInterface diagnostics_interface_{this, "trajectory_safety_filter"}; + DiagnosticsInterface diagnostics_interface_{this, "trajectory_validator"}; }; -} // namespace autoware::trajectory_safety_filter +} // namespace autoware::trajectory_validator -#endif // AUTOWARE__TRAJECTORY_SAFETY_FILTER__TRAJECTORY_SAFETY_FILTER_NODE_HPP_ +#endif // AUTOWARE__TRAJECTORY_VALIDATOR__TRAJECTORY_VALIDATOR_NODE_HPP_ diff --git a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/safety_filter_interface.hpp b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/validator_interface.hpp similarity index 68% rename from planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/safety_filter_interface.hpp rename to planning/autoware_trajectory_validator/include/autoware/trajectory_validator/validator_interface.hpp index d641062d6d94f..fe7e6aa2b22b9 100644 --- a/planning/autoware_trajectory_safety_filter/include/autoware/trajectory_safety_filter/safety_filter_interface.hpp +++ b/planning/autoware_trajectory_validator/include/autoware/trajectory_validator/validator_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY_SAFETY_FILTER__SAFETY_FILTER_INTERFACE_HPP_ -#define AUTOWARE__TRAJECTORY_SAFETY_FILTER__SAFETY_FILTER_INTERFACE_HPP_ +#ifndef AUTOWARE__TRAJECTORY_VALIDATOR__VALIDATOR_INTERFACE_HPP_ +#define AUTOWARE__TRAJECTORY_VALIDATOR__VALIDATOR_INTERFACE_HPP_ -#include "autoware/trajectory_safety_filter/filter_context.hpp" +#include "autoware/trajectory_validator/filter_context.hpp" #include #include @@ -29,22 +29,22 @@ #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using VehicleInfo = autoware::vehicle_info_utils::VehicleInfo; -class SafetyFilterInterface +class ValidatorInterface { public: - explicit SafetyFilterInterface(std::string name) : name_(std::move(name)) {} + explicit ValidatorInterface(std::string name) : name_(std::move(name)) {} - virtual ~SafetyFilterInterface() = default; - SafetyFilterInterface(const SafetyFilterInterface &) = delete; - SafetyFilterInterface & operator=(const SafetyFilterInterface &) = delete; - SafetyFilterInterface(SafetyFilterInterface &&) = delete; - SafetyFilterInterface & operator=(SafetyFilterInterface &&) = delete; + virtual ~ValidatorInterface() = default; + ValidatorInterface(const ValidatorInterface &) = delete; + ValidatorInterface & operator=(const ValidatorInterface &) = delete; + ValidatorInterface(ValidatorInterface &&) = delete; + ValidatorInterface & operator=(ValidatorInterface &&) = delete; // Main filter method with context for plugin-specific data virtual tl::expected is_feasible( @@ -68,7 +68,7 @@ class SafetyFilterInterface std::string name_; std::shared_ptr vehicle_info_ptr_; }; -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin // NOLINTNEXTLINE -#endif // AUTOWARE__TRAJECTORY_SAFETY_FILTER__SAFETY_FILTER_INTERFACE_HPP_ +#endif // AUTOWARE__TRAJECTORY_VALIDATOR__VALIDATOR_INTERFACE_HPP_ diff --git a/planning/autoware_trajectory_safety_filter/launch/trajectory_safety_filter.launch.xml b/planning/autoware_trajectory_validator/launch/trajectory_validator.launch.xml similarity index 68% rename from planning/autoware_trajectory_safety_filter/launch/trajectory_safety_filter.launch.xml rename to planning/autoware_trajectory_validator/launch/trajectory_validator.launch.xml index 88ac22e05a5af..eade3d9b9ebe2 100644 --- a/planning/autoware_trajectory_safety_filter/launch/trajectory_safety_filter.launch.xml +++ b/planning/autoware_trajectory_validator/launch/trajectory_validator.launch.xml @@ -1,6 +1,6 @@ - + @@ -8,8 +8,8 @@ - - + + diff --git a/planning/autoware_trajectory_safety_filter/package.xml b/planning/autoware_trajectory_validator/package.xml similarity index 93% rename from planning/autoware_trajectory_safety_filter/package.xml rename to planning/autoware_trajectory_validator/package.xml index e2cf4d9022bbd..81caf758cd213 100644 --- a/planning/autoware_trajectory_safety_filter/package.xml +++ b/planning/autoware_trajectory_validator/package.xml @@ -1,9 +1,9 @@ - autoware_trajectory_safety_filter + autoware_trajectory_validator 0.1.0 - The autoware_trajectory_safety_filter package + The autoware_trajectory_validator package Daniel Sanchez Yukihiro Saito diff --git a/planning/autoware_trajectory_safety_filter/param/parameter_struct.yaml b/planning/autoware_trajectory_validator/param/parameter_struct.yaml similarity index 98% rename from planning/autoware_trajectory_safety_filter/param/parameter_struct.yaml rename to planning/autoware_trajectory_validator/param/parameter_struct.yaml index 1e9a4446a9017..0751ee163a555 100644 --- a/planning/autoware_trajectory_safety_filter/param/parameter_struct.yaml +++ b/planning/autoware_trajectory_validator/param/parameter_struct.yaml @@ -1,4 +1,4 @@ -safety_filter: +validator: filter_names: type: string_array default_value: [] diff --git a/planning/autoware_trajectory_validator/plugins.xml b/planning/autoware_trajectory_validator/plugins.xml new file mode 100644 index 0000000000000..5e4d6492012ce --- /dev/null +++ b/planning/autoware_trajectory_validator/plugins.xml @@ -0,0 +1,33 @@ + + + + + Filters trajectories that go out of lane boundaries + + + + + Filters trajectories based on Time To Collision (TTC) with predicted objects + + + + + Filters trajectories that cross uncrossable boundaries + + + + + Filters trajectories that violate vehicle constraints such as maximum speed, acceleration, and steering angle + + + + + Filters trajectories based on collision checks with predicted objects + + + diff --git a/planning/autoware_trajectory_safety_filter/src/filters/collision_check_filter.cpp b/planning/autoware_trajectory_validator/src/filters/safety/collision_check_filter.cpp similarity index 97% rename from planning/autoware_trajectory_safety_filter/src/filters/collision_check_filter.cpp rename to planning/autoware_trajectory_validator/src/filters/safety/collision_check_filter.cpp index f79a4474854b9..2ba975d35acc3 100644 --- a/planning/autoware_trajectory_safety_filter/src/filters/collision_check_filter.cpp +++ b/planning/autoware_trajectory_validator/src/filters/safety/collision_check_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory_safety_filter/filters/collision_check_filter.hpp" +#include "autoware/trajectory_validator/filters/safety/collision_check_filter.hpp" #include #include @@ -31,7 +31,7 @@ #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { namespace motion @@ -420,9 +420,9 @@ tl::expected CollisionCheckFilter::is_feasible( return {}; } -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin #include PLUGINLIB_EXPORT_CLASS( - autoware::trajectory_safety_filter::plugin::CollisionCheckFilter, - autoware::trajectory_safety_filter::plugin::SafetyFilterInterface) + autoware::trajectory_validator::plugin::CollisionCheckFilter, + autoware::trajectory_validator::plugin::ValidatorInterface) diff --git a/planning/autoware_trajectory_safety_filter/src/filters/collision_filter.cpp b/planning/autoware_trajectory_validator/src/filters/safety/collision_filter.cpp similarity index 97% rename from planning/autoware_trajectory_safety_filter/src/filters/collision_filter.cpp rename to planning/autoware_trajectory_validator/src/filters/safety/collision_filter.cpp index de8dda6503123..f5b381a234366 100644 --- a/planning/autoware_trajectory_safety_filter/src/filters/collision_filter.cpp +++ b/planning/autoware_trajectory_validator/src/filters/safety/collision_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory_safety_filter/filters/collision_filter.hpp" +#include "autoware/trajectory_validator/filters/safety/collision_filter.hpp" #include @@ -27,7 +27,7 @@ #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { namespace @@ -283,9 +283,9 @@ void CollisionFilter::update_parameters(const std::vector & p update_param(parameters, "collision.time", params_.max_check_time); update_param(parameters, "collision.min_value", params_.min_ttc); } -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin #include PLUGINLIB_EXPORT_CLASS( - autoware::trajectory_safety_filter::plugin::CollisionFilter, - autoware::trajectory_safety_filter::plugin::SafetyFilterInterface) + autoware::trajectory_validator::plugin::CollisionFilter, + autoware::trajectory_validator::plugin::ValidatorInterface) diff --git a/planning/autoware_trajectory_safety_filter/src/filters/out_of_lane_filter.cpp b/planning/autoware_trajectory_validator/src/filters/safety/out_of_lane_filter.cpp similarity index 90% rename from planning/autoware_trajectory_safety_filter/src/filters/out_of_lane_filter.cpp rename to planning/autoware_trajectory_validator/src/filters/safety/out_of_lane_filter.cpp index 98b7c1136ed3c..9810ef7b3392a 100644 --- a/planning/autoware_trajectory_safety_filter/src/filters/out_of_lane_filter.cpp +++ b/planning/autoware_trajectory_validator/src/filters/safety/out_of_lane_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory_safety_filter/filters/out_of_lane_filter.hpp" +#include "autoware/trajectory_validator/filters/safety/out_of_lane_filter.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { namespace @@ -60,7 +60,7 @@ autoware_internal_planning_msgs::msg::PathWithLaneId convert_to_path_with_lane_i } } // namespace -OutOfLaneFilter::OutOfLaneFilter() : SafetyFilterInterface("OutOfLaneFilter") +OutOfLaneFilter::OutOfLaneFilter() : ValidatorInterface("OutOfLaneFilter") { // BoundaryDepartureChecker will be initialized when vehicle_info is set } @@ -109,9 +109,9 @@ tl::expected OutOfLaneFilter::is_feasible( return {}; } -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin #include PLUGINLIB_EXPORT_CLASS( - autoware::trajectory_safety_filter::plugin::OutOfLaneFilter, - autoware::trajectory_safety_filter::plugin::SafetyFilterInterface) + autoware::trajectory_validator::plugin::OutOfLaneFilter, + autoware::trajectory_validator::plugin::ValidatorInterface) diff --git a/planning/autoware_trajectory_safety_filter/src/filters/uncrossable_boundary_departure_filter.cpp b/planning/autoware_trajectory_validator/src/filters/safety/uncrossable_boundary_departure_filter.cpp similarity index 85% rename from planning/autoware_trajectory_safety_filter/src/filters/uncrossable_boundary_departure_filter.cpp rename to planning/autoware_trajectory_validator/src/filters/safety/uncrossable_boundary_departure_filter.cpp index 387219c5aefa0..68687b0354bdc 100644 --- a/planning/autoware_trajectory_safety_filter/src/filters/uncrossable_boundary_departure_filter.cpp +++ b/planning/autoware_trajectory_validator/src/filters/safety/uncrossable_boundary_departure_filter.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory_safety_filter/filters/uncrossable_boundary_departure_filter.hpp" +#include "autoware/trajectory_validator/filters/safety/uncrossable_boundary_departure_filter.hpp" #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { tl::expected UncrossableBoundaryDepartureFilter::is_feasible( const TrajectoryPoints & traj_points, const FilterContext & context) @@ -64,9 +64,9 @@ std::optional UncrossableBoundaryDepartureFilter::is_invalid_input( return std::nullopt; } -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin #include PLUGINLIB_EXPORT_CLASS( - autoware::trajectory_safety_filter::plugin::UncrossableBoundaryDepartureFilter, - autoware::trajectory_safety_filter::plugin::SafetyFilterInterface) + autoware::trajectory_validator::plugin::UncrossableBoundaryDepartureFilter, + autoware::trajectory_validator::plugin::ValidatorInterface) diff --git a/planning/autoware_trajectory_safety_filter/src/filters/vehicle_constraint_filter.cpp b/planning/autoware_trajectory_validator/src/filters/safety/vehicle_constraint_filter.cpp similarity index 95% rename from planning/autoware_trajectory_safety_filter/src/filters/vehicle_constraint_filter.cpp rename to planning/autoware_trajectory_validator/src/filters/safety/vehicle_constraint_filter.cpp index 80686c5df6416..c73924a17f264 100644 --- a/planning/autoware_trajectory_safety_filter/src/filters/vehicle_constraint_filter.cpp +++ b/planning/autoware_trajectory_validator/src/filters/safety/vehicle_constraint_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory_safety_filter/filters/vehicle_constraint_filter.hpp" +#include "autoware/trajectory_validator/filters/safety/vehicle_constraint_filter.hpp" #include #include @@ -20,7 +20,7 @@ #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { namespace { @@ -89,8 +89,7 @@ double to_steering_rate( } } // namespace -VehicleConstraintFilter::VehicleConstraintFilter() -: SafetyFilterInterface("VehicleConstraintFilter") +VehicleConstraintFilter::VehicleConstraintFilter() : ValidatorInterface("VehicleConstraintFilter") { } @@ -260,9 +259,9 @@ bool is_steering_rate_ok( } return true; } -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin #include PLUGINLIB_EXPORT_CLASS( - autoware::trajectory_safety_filter::plugin::VehicleConstraintFilter, - autoware::trajectory_safety_filter::plugin::SafetyFilterInterface) + autoware::trajectory_validator::plugin::VehicleConstraintFilter, + autoware::trajectory_validator::plugin::ValidatorInterface) diff --git a/planning/autoware_trajectory_safety_filter/src/trajectory_safety_filter_node.cpp b/planning/autoware_trajectory_validator/src/trajectory_validator_node.cpp similarity index 82% rename from planning/autoware_trajectory_safety_filter/src/trajectory_safety_filter_node.cpp rename to planning/autoware_trajectory_validator/src/trajectory_validator_node.cpp index f53565cf0dd8d..08d143983295e 100644 --- a/planning/autoware_trajectory_safety_filter/src/trajectory_safety_filter_node.cpp +++ b/planning/autoware_trajectory_validator/src/trajectory_validator_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory_safety_filter/trajectory_safety_filter_node.hpp" +#include "autoware/trajectory_validator/trajectory_validator_node.hpp" -#include "autoware/trajectory_safety_filter/filter_context.hpp" -#include "autoware/trajectory_safety_filter/safety_filter_interface.hpp" +#include "autoware/trajectory_validator/filter_context.hpp" +#include "autoware/trajectory_validator/validator_interface.hpp" #include @@ -61,15 +61,14 @@ bool has_trajectory_from_generator( } } // namespace -namespace autoware::trajectory_safety_filter +namespace autoware::trajectory_validator { -TrajectorySafetyFilter::TrajectorySafetyFilter(const rclcpp::NodeOptions & options) -: Node{"trajectory_safety_filter_node", options}, - listener_{std::make_unique(get_node_parameters_interface())}, +TrajectoryValidator::TrajectoryValidator(const rclcpp::NodeOptions & options) +: Node{"trajectory_validator_node", options}, + listener_{std::make_unique(get_node_parameters_interface())}, plugin_loader_( - "autoware_trajectory_safety_filter", - "autoware::trajectory_safety_filter::plugin::SafetyFilterInterface"), + "autoware_trajectory_validator", "autoware::trajectory_validator::plugin::ValidatorInterface"), vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()) { const auto filters = listener_->get_params().filter_names; @@ -79,11 +78,11 @@ TrajectorySafetyFilter::TrajectorySafetyFilter(const rclcpp::NodeOptions & optio sub_map_ = create_subscription( "~/input/lanelet2_map", rclcpp::QoS{1}.transient_local(), - std::bind(&TrajectorySafetyFilter::map_callback, this, std::placeholders::_1)); + std::bind(&TrajectoryValidator::map_callback, this, std::placeholders::_1)); sub_trajectories_ = create_subscription( "~/input/trajectories", 1, - std::bind(&TrajectorySafetyFilter::process, this, std::placeholders::_1)); + std::bind(&TrajectoryValidator::process, this, std::placeholders::_1)); pub_trajectories_ = create_publisher("~/output/trajectories", 1); @@ -93,10 +92,10 @@ TrajectorySafetyFilter::TrajectorySafetyFilter(const rclcpp::NodeOptions & optio std::make_shared(debug_processing_time_detail_pub_); set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&TrajectorySafetyFilter::on_parameter, this, std::placeholders::_1)); + std::bind(&TrajectoryValidator::on_parameter, this, std::placeholders::_1)); } -void TrajectorySafetyFilter::process(const CandidateTrajectories::ConstSharedPtr msg) +void TrajectoryValidator::process(const CandidateTrajectories::ConstSharedPtr msg) { autoware_utils_debug::ScopedTimeTrack st(__func__, *time_keeper_); @@ -161,7 +160,7 @@ void TrajectorySafetyFilter::process(const CandidateTrajectories::ConstSharedPtr pub_trajectories_->publish(*filtered_msg); } -void TrajectorySafetyFilter::map_callback(const LaneletMapBin::ConstSharedPtr msg) +void TrajectoryValidator::map_callback(const LaneletMapBin::ConstSharedPtr msg) { autoware_utils_debug::ScopedTimeTrack st(__func__, *time_keeper_); @@ -169,7 +168,7 @@ void TrajectorySafetyFilter::map_callback(const LaneletMapBin::ConstSharedPtr ms lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); } -void TrajectorySafetyFilter::load_metric(const std::string & name) +void TrajectoryValidator::load_metric(const std::string & name) { if (name == "") return; @@ -192,19 +191,18 @@ void TrajectorySafetyFilter::load_metric(const std::string & name) get_logger(), "The scene plugin '" << name << "' is loaded and initialized."); } catch (const pluginlib::CreateClassException & e) { RCLCPP_ERROR_STREAM( - get_logger(), - "[safety_filter] createSharedInstance failed for '" << name << "': " << e.what()); + get_logger(), "[validator] createSharedInstance failed for '" << name << "': " << e.what()); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM( - get_logger(), "[safety_filter] unexpected exception for '" << name << "': " << e.what()); + get_logger(), "[validator] unexpected exception for '" << name << "': " << e.what()); } } -void TrajectorySafetyFilter::unload_metric(const std::string & name) +void TrajectoryValidator::unload_metric(const std::string & name) { auto it = std::remove_if( plugins_.begin(), plugins_.end(), - [&](const std::shared_ptr & plugin) { + [&](const std::shared_ptr & plugin) { return plugin->get_name() == name; }); @@ -217,7 +215,7 @@ void TrajectorySafetyFilter::unload_metric(const std::string & name) } } -void TrajectorySafetyFilter::update_diagnostic( +void TrajectoryValidator::update_diagnostic( const CandidateTrajectories & input_trajectories, const CandidateTrajectories & filtered_trajectories) { @@ -242,7 +240,7 @@ void TrajectorySafetyFilter::update_diagnostic( diagnostics_interface_.publish(this->get_clock()->now()); } -rcl_interfaces::msg::SetParametersResult TrajectorySafetyFilter::on_parameter( +rcl_interfaces::msg::SetParametersResult TrajectoryValidator::on_parameter( const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; @@ -262,7 +260,7 @@ rcl_interfaces::msg::SetParametersResult TrajectorySafetyFilter::on_parameter( return result; } -} // namespace autoware::trajectory_safety_filter +} // namespace autoware::trajectory_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::trajectory_safety_filter::TrajectorySafetyFilter) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::trajectory_validator::TrajectoryValidator) diff --git a/planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_collision.cpp b/planning/autoware_trajectory_validator/test/collision_check_filter/test_collision.cpp similarity index 97% rename from planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_collision.cpp rename to planning/autoware_trajectory_validator/test/collision_check_filter/test_collision.cpp index d4066c8d08e03..872d0f516b5a4 100644 --- a/planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_collision.cpp +++ b/planning/autoware_trajectory_validator/test/collision_check_filter/test_collision.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "../..//src/filters/collision_check_filter.cpp" +#include "../..//src/filters/safety/collision_check_filter.cpp" #include @@ -20,7 +20,7 @@ #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { class CollisionCheckFilterTest : public ::testing::Test @@ -227,4 +227,4 @@ TEST_F(CollisionCheckFilterTest, ObjectWillEnterPath) << "Error string did not contain 'constant_curvature_path'. Actual: " << error_msg; } -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin diff --git a/planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_constant_curvature_predictor.cpp b/planning/autoware_trajectory_validator/test/collision_check_filter/test_constant_curvature_predictor.cpp similarity index 95% rename from planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_constant_curvature_predictor.cpp rename to planning/autoware_trajectory_validator/test/collision_check_filter/test_constant_curvature_predictor.cpp index f0f8a304110f5..0e956a59ab3e9 100644 --- a/planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_constant_curvature_predictor.cpp +++ b/planning/autoware_trajectory_validator/test/collision_check_filter/test_constant_curvature_predictor.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -// #include "autoware/trajectory_safety_filter/filters/collision_check_filter.cpp" -#include "../..//src/filters/collision_check_filter.cpp" +// #include "autoware/trajectory_validator/filters/safety/collision_check_filter.cpp" +#include "../..//src/filters/safety/collision_check_filter.cpp" #include @@ -23,7 +23,7 @@ #include -namespace autoware::trajectory_safety_filter::plugin::constant_curvature_predictor +namespace autoware::trajectory_validator::plugin::constant_curvature_predictor { TEST(ConstantCurvaturePoseTrajectoryCalculatorTest, PoseToIsometry) @@ -216,4 +216,4 @@ TEST(ConstantCurvaturePoseTrajectoryCalculatorTest, ComputeTrajectory) } } -} // namespace autoware::trajectory_safety_filter::plugin::constant_curvature_predictor +} // namespace autoware::trajectory_validator::plugin::constant_curvature_predictor diff --git a/planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_motion.cpp b/planning/autoware_trajectory_validator/test/collision_check_filter/test_motion.cpp similarity index 93% rename from planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_motion.cpp rename to planning/autoware_trajectory_validator/test/collision_check_filter/test_motion.cpp index d56ace55c074c..d1e1a16a5fa60 100644 --- a/planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_motion.cpp +++ b/planning/autoware_trajectory_validator/test/collision_check_filter/test_motion.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -// #include "autoware/trajectory_safety_filter/filters/collision_check_filter.hpp" -#include "../../src/filters/collision_check_filter.cpp" +// #include "autoware/trajectory_validator/filters/safety/collision_check_filter.hpp" +#include "../../src/filters/safety/collision_check_filter.cpp" #include #include #include -namespace autoware::trajectory_safety_filter::plugin::motion +namespace autoware::trajectory_validator::plugin::motion { class ComputeMotionProfile1dTest : public ::testing::Test { @@ -127,4 +127,4 @@ TEST_F(ComputeMotionProfile1dTest, LagLongerThanMaxTime) } } -} // namespace autoware::trajectory_safety_filter::plugin::motion +} // namespace autoware::trajectory_validator::plugin::motion diff --git a/planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_polygon.cpp b/planning/autoware_trajectory_validator/test/collision_check_filter/test_polygon.cpp similarity index 92% rename from planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_polygon.cpp rename to planning/autoware_trajectory_validator/test/collision_check_filter/test_polygon.cpp index c239f7e1b89f1..978dcfba4a81c 100644 --- a/planning/autoware_trajectory_safety_filter/test/collision_check_filter/test_polygon.cpp +++ b/planning/autoware_trajectory_validator/test/collision_check_filter/test_polygon.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -// #include "autoware/trajectory_safety_filter/filters/collision_check_filter.hpp" -#include "../../src/filters/collision_check_filter.cpp" +// #include "autoware/trajectory_validator/filters/safety/collision_check_filter.hpp" +#include "../../src/filters/safety/collision_check_filter.cpp" #include #include #include -namespace autoware::trajectory_safety_filter::plugin::polygon +namespace autoware::trajectory_validator::plugin::polygon { class PolygonCollisionTest : public ::testing::Test @@ -95,4 +95,4 @@ TEST_F(PolygonCollisionTest, CheckPathPolygonConvexCollision) EXPECT_TRUE(check_path_polygon_convex_collision(traj1_collide, traj2_collide)); } -} // namespace autoware::trajectory_safety_filter::plugin::polygon +} // namespace autoware::trajectory_validator::plugin::polygon diff --git a/planning/autoware_trajectory_validator/test/dummy_plugin/dummy_plugins.xml b/planning/autoware_trajectory_validator/test/dummy_plugin/dummy_plugins.xml new file mode 100644 index 0000000000000..15f5b710dd034 --- /dev/null +++ b/planning/autoware_trajectory_validator/test/dummy_plugin/dummy_plugins.xml @@ -0,0 +1,7 @@ + + + + A predictable dummy filter for testing the main node + + diff --git a/planning/autoware_trajectory_safety_filter/test/dummy_plugin/dummy_safety_filter_plugin.cpp b/planning/autoware_trajectory_validator/test/dummy_plugin/dummy_validator_plugin.cpp similarity index 80% rename from planning/autoware_trajectory_safety_filter/test/dummy_plugin/dummy_safety_filter_plugin.cpp rename to planning/autoware_trajectory_validator/test/dummy_plugin/dummy_validator_plugin.cpp index 11f4284c4824a..f5da10a814e9c 100644 --- a/planning/autoware_trajectory_safety_filter/test/dummy_plugin/dummy_safety_filter_plugin.cpp +++ b/planning/autoware_trajectory_validator/test/dummy_plugin/dummy_validator_plugin.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory_safety_filter/safety_filter_interface.hpp" +#include "autoware/trajectory_validator/validator_interface.hpp" #include #include #include -namespace autoware::trajectory_safety_filter::plugin +namespace autoware::trajectory_validator::plugin { -class DummyFilter : public SafetyFilterInterface +class DummyFilter : public ValidatorInterface { struct DummyFilterParam { @@ -29,7 +29,7 @@ class DummyFilter : public SafetyFilterInterface }; public: - DummyFilter() : SafetyFilterInterface("DummyFilter") {} + DummyFilter() : ValidatorInterface("DummyFilter") {} tl::expected is_feasible( const TrajectoryPoints & traj_points, const FilterContext & /*context*/) final @@ -62,8 +62,8 @@ class DummyFilter : public SafetyFilterInterface private: DummyFilterParam params_; }; -} // namespace autoware::trajectory_safety_filter::plugin +} // namespace autoware::trajectory_validator::plugin PLUGINLIB_EXPORT_CLASS( - autoware::trajectory_safety_filter::plugin::DummyFilter, - autoware::trajectory_safety_filter::plugin::SafetyFilterInterface) + autoware::trajectory_validator::plugin::DummyFilter, + autoware::trajectory_validator::plugin::ValidatorInterface) diff --git a/planning/autoware_trajectory_safety_filter/test/node/test_trajectory_safety_filter_node.cpp b/planning/autoware_trajectory_validator/test/node/test_trajectory_validator_node.cpp similarity index 85% rename from planning/autoware_trajectory_safety_filter/test/node/test_trajectory_safety_filter_node.cpp rename to planning/autoware_trajectory_validator/test/node/test_trajectory_validator_node.cpp index d16afb1fcd40e..3d5dd66dda4af 100644 --- a/planning/autoware_trajectory_safety_filter/test/node/test_trajectory_safety_filter_node.cpp +++ b/planning/autoware_trajectory_validator/test/node/test_trajectory_validator_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory_safety_filter/trajectory_safety_filter_node.hpp" +#include "autoware/trajectory_validator/trajectory_validator_node.hpp" #include #include @@ -23,10 +23,10 @@ #include #include -namespace autoware::trajectory_safety_filter +namespace autoware::trajectory_validator { -class TrajectorySafetyFilterNodeTest : public ::testing::Test +class TrajectoryValidatorNodeTest : public ::testing::Test { protected: void SetUp() override @@ -34,7 +34,7 @@ class TrajectorySafetyFilterNodeTest : public ::testing::Test rclcpp::init(0, nullptr); node_options_.append_parameter_override( "filter_names", - std::vector{"autoware::trajectory_safety_filter::plugin::DummyFilter"}); + std::vector{"autoware::trajectory_validator::plugin::DummyFilter"}); node_options_.append_parameter_override("dummy.dummy_param", 0.0); const auto test_pkg_share = ament_index_cpp::get_package_share_directory("autoware_test_utils"); @@ -43,25 +43,25 @@ class TrajectorySafetyFilterNodeTest : public ::testing::Test autoware::test_utils::updateNodeOptions(node_options_, {vehicle_info_param_path}); - node_under_test_ = std::make_shared(node_options_); + node_under_test_ = std::make_shared(node_options_); test_node_ = std::make_shared("test_helper_node"); map_pub_ = test_node_->create_publisher( - "/trajectory_safety_filter_node/input/lanelet2_map", rclcpp::QoS{1}.transient_local()); + "/trajectory_validator_node/input/lanelet2_map", rclcpp::QoS{1}.transient_local()); odom_pub_ = test_node_->create_publisher( - "/trajectory_safety_filter_node/input/odometry", 1); + "/trajectory_validator_node/input/odometry", 1); accel_pub_ = test_node_->create_publisher( - "/trajectory_safety_filter_node/input/acceleration", 1); + "/trajectory_validator_node/input/acceleration", 1); obj_pub_ = test_node_->create_publisher( - "/trajectory_safety_filter_node/input/objects", 1); + "/trajectory_validator_node/input/objects", 1); traj_pub_ = test_node_->create_publisher( - "/trajectory_safety_filter_node/input/trajectories", 1); + "/trajectory_validator_node/input/trajectories", 1); output_sub_ = test_node_->create_subscription( - "/trajectory_safety_filter_node/output/trajectories", 1, + "/trajectory_validator_node/output/trajectories", 1, [this]( const autoware_internal_planning_msgs::msg::CandidateTrajectories::ConstSharedPtr msg) { last_output_ = msg; @@ -126,7 +126,7 @@ class TrajectorySafetyFilterNodeTest : public ::testing::Test rclcpp::NodeOptions node_options_; rclcpp::Node::SharedPtr test_node_; - std::shared_ptr node_under_test_; + std::shared_ptr node_under_test_; rclcpp::Publisher::SharedPtr map_pub_; rclcpp::Publisher::SharedPtr odom_pub_; @@ -140,7 +140,7 @@ class TrajectorySafetyFilterNodeTest : public ::testing::Test autoware_internal_planning_msgs::msg::CandidateTrajectories::ConstSharedPtr last_output_; }; -TEST_F(TrajectorySafetyFilterNodeTest, FiltersTrajectoriesViaPlugin) +TEST_F(TrajectoryValidatorNodeTest, FiltersTrajectoriesViaPlugin) { publish_context(); spin_until([] { return false; }, std::chrono::milliseconds(100)); @@ -160,7 +160,7 @@ TEST_F(TrajectorySafetyFilterNodeTest, FiltersTrajectoriesViaPlugin) EXPECT_EQ(last_output_->generator_info.front().generator_name.data, "SafePlanner"); } -TEST_F(TrajectorySafetyFilterNodeTest, UpdateParametersDynamically) +TEST_F(TrajectoryValidatorNodeTest, UpdateParametersDynamically) { publish_context(); spin_until([] { return false; }, std::chrono::milliseconds(100)); @@ -183,7 +183,7 @@ TEST_F(TrajectorySafetyFilterNodeTest, UpdateParametersDynamically) EXPECT_EQ(last_output_->candidate_trajectories.size(), 1u); } -TEST_F(TrajectorySafetyFilterNodeTest, HandlesPluginRejection) +TEST_F(TrajectoryValidatorNodeTest, HandlesPluginRejection) { publish_context(); spin_until([] { return false; }, std::chrono::milliseconds(100)); @@ -196,4 +196,4 @@ TEST_F(TrajectorySafetyFilterNodeTest, HandlesPluginRejection) ASSERT_TRUE(spin_until([this] { return last_output_ != nullptr; })); EXPECT_EQ(last_output_->candidate_trajectories.size(), 0u); } -} // namespace autoware::trajectory_safety_filter +} // namespace autoware::trajectory_validator diff --git a/planning/autoware_trajectory_safety_filter/test/plugins/test_vehicle_constraint_filter.cpp b/planning/autoware_trajectory_validator/test/plugins/test_vehicle_constraint_filter.cpp similarity index 98% rename from planning/autoware_trajectory_safety_filter/test/plugins/test_vehicle_constraint_filter.cpp rename to planning/autoware_trajectory_validator/test/plugins/test_vehicle_constraint_filter.cpp index cd5526cf8f5c9..fb275c961b9df 100644 --- a/planning/autoware_trajectory_safety_filter/test/plugins/test_vehicle_constraint_filter.cpp +++ b/planning/autoware_trajectory_validator/test/plugins/test_vehicle_constraint_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory_safety_filter/filters/vehicle_constraint_filter.hpp" +#include "autoware/trajectory_validator/filters/safety/vehicle_constraint_filter.hpp" #include @@ -36,7 +36,7 @@ autoware_planning_msgs::msg::TrajectoryPoint create_trajectory_point( } } // namespace -namespace autoware::trajectory_safety_filter::plugin::testing +namespace autoware::trajectory_validator::plugin::testing { TEST(VehicleConstraintFilterTest, FeasibleWhenAllConstraintsSatisfied) { @@ -308,4 +308,4 @@ TEST(IsSteeringRateOkTest, FalseWhenAnySteeringRateAboveMax) EXPECT_FALSE(is_steering_rate_ok(traj_points, vehicle_info, max_steering_rate)); } -} // namespace autoware::trajectory_safety_filter::plugin::testing +} // namespace autoware::trajectory_validator::plugin::testing