From b469d3ffe3585141e9643c432f6c7218d293cd3b Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 18 Jun 2024 17:56:57 +0900 Subject: [PATCH] cleanup velocity factors, launch param, and remove .pages Signed-off-by: Maxime CLEMENT --- .../factor/velocity_factor_interface.hpp | 52 ----------- .../src/factor/velocity_factor_interface.cpp | 49 ---------- .../motion_planning/motion_planning.launch.py | 2 +- planning/.pages | 89 ------------------- .../src/out_of_lane_module.cpp | 13 +-- .../plugin_module_interface.hpp | 2 - .../src/node.hpp | 1 + 7 files changed, 9 insertions(+), 199 deletions(-) delete mode 100644 common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp delete mode 100644 common/motion_utils/src/factor/velocity_factor_interface.cpp delete mode 100644 planning/.pages diff --git a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp deleted file mode 100644 index a0046b7acd162..0000000000000 --- a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp +++ /dev/null @@ -1,52 +0,0 @@ - -// Copyright 2022-2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ -#define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ - -#include -#include -#include - -#include -#include - -namespace motion_utils -{ -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using VelocityFactorBehavior = VelocityFactor::_type_type; -using VelocityFactorStatus = VelocityFactor::_status_type; -using geometry_msgs::msg::Pose; - -class VelocityFactorInterface -{ -public: - [[nodiscard]] VelocityFactor get() const { return velocity_factor_; } - void init(const VelocityFactorBehavior & behavior) { type_ = behavior; } - void reset() { velocity_factor_.type = VelocityFactor::UNKNOWN; } - - template - void set( - const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, - const VelocityFactorStatus status, const std::string & detail = ""); - -private: - VelocityFactorBehavior type_{VelocityFactor::UNKNOWN}; - VelocityFactor velocity_factor_{}; -}; - -} // namespace motion_utils - -#endif // MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/motion_utils/src/factor/velocity_factor_interface.cpp deleted file mode 100644 index 6f1b987721061..0000000000000 --- a/common/motion_utils/src/factor/velocity_factor_interface.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2023-2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include -#include - -namespace motion_utils -{ -template -void VelocityFactorInterface::set( - const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, - const VelocityFactorStatus status, const std::string & detail) -{ - const auto & curr_point = curr_pose.position; - const auto & stop_point = stop_pose.position; - velocity_factor_.type = type_; - velocity_factor_.pose = stop_pose; - velocity_factor_.distance = - static_cast(motion_utils::calcSignedArcLength(points, curr_point, stop_point)); - velocity_factor_.status = status; - velocity_factor_.detail = detail; -} - -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, - const Pose &, const VelocityFactorStatus, const std::string &); -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, - const VelocityFactorStatus, const std::string &); -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, - const Pose &, const VelocityFactorStatus, const std::string &); - -} // namespace motion_utils diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index d695d0081a91e..f3fb1bf3d3ebf 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -154,7 +154,7 @@ def launch_setup(context, *args, **kwargs): ) as f: motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open( - LaunchConfiguration("behavior_velocity_smoother_type_param_path").perform(context), "r" + LaunchConfiguration("motion_velocity_smoother_type_param_path").perform(context), "r" ) as f: behavior_velocity_smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open( diff --git a/planning/.pages b/planning/.pages deleted file mode 100644 index 4514894605bbc..0000000000000 --- a/planning/.pages +++ /dev/null @@ -1,89 +0,0 @@ -nav: - - 'Introduction': planning - - 'Behavior Path Planner': - - 'About Behavior Path': planning/behavior_path_planner - - 'Concept': - - 'Planner Manager': planning/behavior_path_planner/docs/behavior_path_planner_manager_design - - 'Scene Module Interface': planning/behavior_path_planner/docs/behavior_path_planner_interface_design - - 'Path Generation': planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design - - 'Collision Assessment/Safety Check': planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check - - 'Dynamic Drivable Area': planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design - - 'Turn Signal': planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design - - 'Scene Module': - - 'Avoidance': planning/behavior_path_avoidance_module - - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module - - 'Dynamic Avoidance': planning/behavior_path_dynamic_avoidance_module - - 'Goal Planner': planning/behavior_path_goal_planner_module - - 'Lane Change': planning/behavior_path_lane_change_module - - 'Side Shift': planning/behavior_path_side_shift_module - - 'Start Planner': planning/behavior_path_start_planner_module - - 'Behavior Velocity Planner': - - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner - - 'Template for Custom Module': planning/behavior_velocity_template_module - - 'Available Module': - - 'Blind Spot': planning/behavior_velocity_blind_spot_module - - 'Crosswalk': planning/behavior_velocity_crosswalk_module - - 'Detection Area': planning/behavior_velocity_detection_area_module - - 'Dynamic Obstacle Stop': planning/behavior_velocity_dynamic_obstacle_stop_module - - 'Intersection': planning/behavior_velocity_intersection_module - - 'No Drivable Lane': planning/behavior_velocity_no_drivable_lane_module - - 'No Stopping Area': planning/behavior_velocity_no_stopping_area_module - - 'Occlusion Spot': planning/behavior_velocity_occlusion_spot_module - - 'Out of Lane': planning/behavior_velocity_out_of_lane_module - - 'Run Out': planning/behavior_velocity_run_out_module - - 'Speed Bump': planning/behavior_velocity_speed_bump_module - - 'Stop Line': planning/behavior_velocity_stop_line_module - - 'Traffic Light': planning/behavior_velocity_traffic_light_module - - 'Virtual Traffic Light': planning/behavior_velocity_virtual_traffic_light_module - - 'Walkway': planning/behavior_velocity_walkway_module - - 'Parking': - - 'Freespace Planner': - - 'About Freespace Planner': planning/freespace_planner - - 'Algorithm': planning/freespace_planning_algorithms - - 'RRT*': planning/freespace_planning_algorithms/rrtstar - - 'Mission Planner': planning/mission_planner - - 'Motion Planning': - - 'Obstacle Avoidance Planner': - - 'About Obstacle Avoidance': planning/obstacle_avoidance_planner - - 'Model Predictive Trajectory (MPT)': planning/obstacle_avoidance_planner/docs/mpt - - 'How to Debug': planning/obstacle_avoidance_planner/docs/debug - - 'Obstacle Cruise Planner': - - 'About Obstacle Cruise': planning/obstacle_cruise_planner - - 'How to Debug': planning/obstacle_cruise_planner/docs/debug - - 'Obstacle Stop Planner': planning/obstacle_stop_planner - - 'Obstacle Velocity Limiter': planning/obstacle_velocity_limiter - - 'Path Smoother': - - 'About Path Smoother': planning/path_smoother - - 'Elastic Band': planning/path_smoother/docs/eb - - 'Sampling Based Planner': - - 'Path Sample': planning/sampling_based_planner/path_sampler - - 'Common library': planning/sampling_based_planner/sampler_common - - 'Frenet Planner': planning/sampling_based_planner/frenet_planner - - 'Bezier Sampler': planning/sampling_based_planner/bezier_sampler - - 'Surround Obstacle Checker': - - 'About Surround Obstacle Checker': planning/surround_obstacle_checker - - 'About Surround Obstacle Checker (Japanese)': planning/surround_obstacle_checker/surround_obstacle_checker-design.ja - - 'Motion Velocity Planner': - - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ - - 'Available Modules': - - 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/ - - 'Motion Velocity Smoother': - - 'About Motion Velocity Smoother': planning/motion_velocity_smoother - - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja - - 'Scenario Selector': planning/scenario_selector - - 'Static Centerline Generator': planning/autoware_static_centerline_generator - - 'API and Library': - - 'Costmap Generator': planning/costmap_generator - - 'External Velocity Limit Selector': planning/external_velocity_limit_selector - - 'Objects of Interest Marker Interface': planning/objects_of_interest_marker_interface - - 'Route Handler': planning/route_handler - - 'RTC Interface': planning/rtc_interface - - 'Additional Tools': - - 'RTC Replayer': planning/rtc_replayer - - 'Planning Debug Tools': - - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools - - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md - - 'Planning Test Utils': planning/planning_test_utils - - 'Planning Test Manager': planning/autoware_planning_test_manager - - 'Planning Topic Converter': planning/planning_topic_converter - - 'Planning Validator': planning/planning_validator diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 4c6ba34b8a48e..e873253f32c40 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -50,7 +50,6 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) logger_ = node.get_logger(); clock_ = node.get_clock(); init_parameters(node); - velocity_factor_interface_.init(motion_utils::VelocityFactor::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -265,11 +264,13 @@ VelocityPlanningResult OutOfLaneModule::plan( ego_trajectory_points, ego_data.pose.position, point_to_insert->point.pose.position) > 0.1 && ego_data.velocity > 0.1; - const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING - : motion_utils::VelocityFactor::STOPPED; - // velocity_factor_interface_.set( - // ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane"); - // result.velocity_factor = velocity_factor_interface_.get(); + const auto status = + is_approaching ? result.velocity_factor->APPROACHING : result.velocity_factor->STOPPED; + result.velocity_factor.emplace(); + result.velocity_factor->pose = point_to_insert->point.pose; + result.velocity_factor->status = status; + result.velocity_factor->type = result.velocity_factor->ROUTE_OBSTACLE; + result.velocity_factor->detail = "out_of_lane"; } else if (!decisions.empty()) { RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp index 9011d3fb7e079..d3e4aebec146f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp @@ -18,7 +18,6 @@ #include "planner_data.hpp" #include "velocity_planning_result.hpp" -#include #include #include @@ -40,7 +39,6 @@ class PluginModuleInterface const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; - motion_utils::VelocityFactorInterface velocity_factor_interface_; rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 2f20669ded687..511f949ec2776 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include