diff --git a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml index 1b7510c2ced69..4f36a614b5d09 100644 --- a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 0e693943a4d03..97bf6414189ab 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -33,7 +33,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 51f0a32284626..29fe7f7071126 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -242,7 +242,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index f8deccb188b71..24006e7df580e 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -60,18 +60,18 @@ - + - + - + - + @@ -84,7 +84,7 @@ - + @@ -101,7 +101,7 @@ - + @@ -114,7 +114,7 @@ - + @@ -131,8 +131,8 @@ - - + + @@ -146,7 +146,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 0a30204ca3c99..be85ee704ff95 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -21,22 +21,22 @@ - + - - - + + + - + - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 9d39c397f9417..cf96cd39043ce 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -58,7 +58,9 @@ autoware_cmake autoware_behavior_velocity_planner + autoware_path_optimizer autoware_remaining_distance_time_calculator + autoware_velocity_smoother behavior_path_planner costmap_generator external_cmd_selector @@ -66,8 +68,6 @@ freespace_planner glog_component mission_planner - motion_velocity_smoother - obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner planning_evaluator diff --git a/planning/.pages b/planning/.pages index c886c6a027753..a9a9e3dd7e55b 100644 --- a/planning/.pages +++ b/planning/.pages @@ -43,10 +43,10 @@ nav: - '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 + - 'Path Optimizer': + - 'About Path Optimizer': planning/autoware_path_optimizer + - 'Model Predictive Trajectory (MPT)': planning/autoware_path_optimizer/docs/mpt + - 'How to Debug': planning/autoware_path_optimizer/docs/debug - 'Obstacle Cruise Planner': - 'About Obstacle Cruise': planning/obstacle_cruise_planner - 'How to Debug': planning/obstacle_cruise_planner/docs/debug diff --git a/planning/README.md b/planning/README.md index 0302d37a1180b..ccf8288df3911 100644 --- a/planning/README.md +++ b/planning/README.md @@ -105,7 +105,7 @@ Interested in joining our meetings? We’d love to have you! For more informatio Occasionally, we publish papers specific to the Planning Component in Autoware. We encourage you to explore these publications and find valuable insights for your work. If you find them useful and incorporate any of our methodologies or algorithms in your projects, citing our papers would be immensely helpful. This support allows us to reach a broader audience and continue contributing to the field. -If you use the Jerk Constrained Velocity Planning algorithm in [Motion Velocity Smoother](./motion_velocity_smoother/README.md) module in the Planning Component, we kindly request you to cite the relevant paper. +If you use the Jerk Constrained Velocity Planning algorithm in [Motion Velocity Smoother](./autoware_velocity_smoother/README.md) module in the Planning Component, we kindly request you to cite the relevant paper. diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md index e0fec11d166a6..0b29d463bb105 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md @@ -4,7 +4,7 @@ This module is under development. ## Purpose / Role -This module provides avoidance functions for vehicles, pedestrians, and obstacles in the vicinity of the ego's path in combination with the [obstacle_avoidance_planner](https://autowarefoundation.github.io/autoware.universe/main/planning/obstacle_avoidance_planner/). +This module provides avoidance functions for vehicles, pedestrians, and obstacles in the vicinity of the ego's path in combination with the [autoware_path_optimizer](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_path_optimizer/). Each module performs the following roles. Dynamic Avoidance module cuts off the drivable area according to the position and velocity of the target to be avoided. Obstacle Avoidance module modifies the path to be followed so that it fits within the received drivable area. diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index c97a83fbff644..a9a096dae7d59 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -37,6 +37,7 @@ autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_velocity_smoother behavior_velocity_planner_common diagnostic_msgs eigen @@ -44,7 +45,6 @@ lanelet2_extension libboost-dev motion_utils - motion_velocity_smoother pcl_conversions pluginlib rclcpp diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index d4ae3897bca65..dbbbe143658f4 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -14,11 +14,11 @@ #include "node.hpp" +#include #include #include #include #include -#include #include #include @@ -310,7 +310,7 @@ void BehaviorVelocityPlannerNode::onParam() // constructed. It would be required if it was a callback. std::lock_guard // lock(mutex_); planner_data_.velocity_smoother_ = - std::make_unique(*this); + std::make_unique(*this); planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); } diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 06c1bc48cf529..3f60c904d6eb4 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -51,8 +51,8 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_velocity_planner_dir = ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); - const auto motion_velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); const auto get_behavior_velocity_module_config = [](const std::string & module) { const auto package_name = "behavior_velocity_" + module + "_module"; @@ -83,27 +83,26 @@ std::shared_ptr generateNode() node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", - motion_velocity_smoother_dir + "/config/Analytical.param.yaml", - behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config("blind_spot"), - get_behavior_velocity_module_config("crosswalk"), - get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config("detection_area"), - get_behavior_velocity_module_config("intersection"), - get_behavior_velocity_module_config("no_stopping_area"), - get_behavior_velocity_module_config("occlusion_spot"), - get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config("speed_bump"), - get_behavior_velocity_module_config("stop_line"), - get_behavior_velocity_module_config("traffic_light"), - get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("out_of_lane"), - get_behavior_velocity_module_config("no_drivable_lane")}); + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", + get_behavior_velocity_module_config("blind_spot"), + get_behavior_velocity_module_config("crosswalk"), + get_behavior_velocity_module_config("walkway"), + get_behavior_velocity_module_config("detection_area"), + get_behavior_velocity_module_config("intersection"), + get_behavior_velocity_module_config("no_stopping_area"), + get_behavior_velocity_module_config("occlusion_spot"), + get_behavior_velocity_module_config("run_out"), + get_behavior_velocity_module_config("speed_bump"), + get_behavior_velocity_module_config("stop_line"), + get_behavior_velocity_module_config("traffic_light"), + get_behavior_velocity_module_config("virtual_traffic_light"), + get_behavior_velocity_module_config("out_of_lane"), + get_behavior_velocity_module_config("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/autoware_path_optimizer/CMakeLists.txt similarity index 77% rename from planning/obstacle_avoidance_planner/CMakeLists.txt rename to planning/autoware_path_optimizer/CMakeLists.txt index 2662555d92341..3ceeae1022b10 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/autoware_path_optimizer/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(obstacle_avoidance_planner) +project(autoware_path_optimizer) include(CheckCXXCompilerFlag) @@ -17,7 +17,7 @@ autoware_package() find_package(Eigen3 REQUIRED) -ament_auto_add_library(obstacle_avoidance_planner SHARED +ament_auto_add_library(autoware_path_optimizer SHARED # node src/node.cpp # core algorithms @@ -34,20 +34,20 @@ ament_auto_add_library(obstacle_avoidance_planner SHARED src/utils/geometry_utils.cpp ) -target_include_directories(obstacle_avoidance_planner +target_include_directories(autoware_path_optimizer SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) # register node -rclcpp_components_register_node(obstacle_avoidance_planner - PLUGIN "obstacle_avoidance_planner::ObstacleAvoidancePlanner" - EXECUTABLE obstacle_avoidance_planner_node +rclcpp_components_register_node(autoware_path_optimizer + PLUGIN "autoware_path_optimizer::PathOptimizer" + EXECUTABLE path_optimizer_node ) if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_${PROJECT_NAME}_node_interface.cpp + test/test_path_optimizer_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} diff --git a/planning/obstacle_avoidance_planner/README.md b/planning/autoware_path_optimizer/README.md similarity index 97% rename from planning/obstacle_avoidance_planner/README.md rename to planning/autoware_path_optimizer/README.md index c079a57c3a2da..29ffee76b0c49 100644 --- a/planning/obstacle_avoidance_planner/README.md +++ b/planning/autoware_path_optimizer/README.md @@ -1,4 +1,4 @@ -# Obstacle Avoidance Planner +# Path optimizer ## Purpose @@ -158,7 +158,7 @@ Therefore, we have to make sure that the optimized trajectory is inside the driv - Computation cost is sometimes high. - Because of the approximation such as linearization, some narrow roads cannot be run by the planner. -- Roles of planning for `behavior_path_planner` and `obstacle_avoidance_planner` are not decided clearly. Both can avoid obstacles. +- Roles of planning for `behavior_path_planner` and `path_optimizer` are not decided clearly. Both can avoid obstacles. ## Comparison to other methods @@ -195,7 +195,7 @@ Although it has a cons to converge to the local minima, it can get a good soluti - The point on the vehicle, offset forward with this parameter from the base link` tries to follow the reference path. - change or tune the method to approximate footprints with a set of circles. - - See [here](https://autowarefoundation.github.io/autoware.universe/main/planning/obstacle_avoidance_planner/docs/mpt/#collision-free) + - See [here](https://autowarefoundation.github.io/autoware.universe/main/planning/path_optimizer/docs/mpt/#collision-free) - Tuning means changing the ratio of circle's radius. ### Computation time diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/autoware_path_optimizer/config/path_optimizer.param.yaml similarity index 100% rename from planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml rename to planning/autoware_path_optimizer/config/path_optimizer.param.yaml diff --git a/planning/obstacle_avoidance_planner/docs/debug.md b/planning/autoware_path_optimizer/docs/debug.md similarity index 87% rename from planning/obstacle_avoidance_planner/docs/debug.md rename to planning/autoware_path_optimizer/docs/debug.md index 293f5b2ec1a40..1c07d29650a6a 100644 --- a/planning/obstacle_avoidance_planner/docs/debug.md +++ b/planning/autoware_path_optimizer/docs/debug.md @@ -7,7 +7,7 @@ The visualization markers of the planning flow (Input, Model Predictive Trajecto All the following markers can be visualized by ```bash -ros2 launch obstacle_avoidance_planner launch_visualiation.launch.xml vehilce_model:=sample_vehicle +ros2 launch autoware_path_optimizer launch_visualiation.launch.xml vehilce_model:=sample_vehicle ``` The `vehicle_model` must be specified to make footprints with vehicle's size. @@ -33,7 +33,7 @@ The `vehicle_model` must be specified to make footprints with vehicle's size. - The skyblue left and right line strings, that is visualized by default. - NOTE: - Check if the path is almost inside the drivable area. - - Then, the `obstacle_avoidance_planner` will try to make the trajectory fully inside the drivable area. + - Then, the `path_optimizer` will try to make the trajectory fully inside the drivable area. - During avoidance or lane change by the `behavior` planner, please make sure that the drivable area is expanded correctly. ![drivable_area](../media/debug/drivable_area_visualization.png) @@ -57,7 +57,7 @@ The `vehicle_model` must be specified to make footprints with vehicle's size. - **Vehicle Circles** - The vehicle's shape is represented by a set of circles. - - The `obstacle_avoidance_planner` will try to make the these circles inside the above boundaries' width. + - The `path_optimizer` will try to make the these circles inside the above boundaries' width. ![vehicle_circles](../media/debug/vehicle_circles_visualization.png) @@ -88,7 +88,7 @@ The `vehicle_model` must be specified to make footprints with vehicle's size. ## Calculation time -The `obstacle_avoidance_planner` consists of many functions such as boundaries' width calculation, collision-free planning, etc. +The `path_optimizer` consists of many functions such as boundaries' width calculation, collision-free planning, etc. We can see the calculation time for each function as follows. ### Raw data @@ -96,7 +96,7 @@ We can see the calculation time for each function as follows. Enable `option.enable_calculation_time_info` or echo the topic as follows. ```sh -$ ros2 topic echo /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/calculation_time --field data +$ ros2 topic echo /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/calculation_time --field data --- insertFixedPoint:= 0.008 [ms] getPaddedTrajectoryPoints:= 0.002 [ms] @@ -135,7 +135,7 @@ onPath:= 20.737 [ms] With the following script, any calculation time of the above functions can be plot. ```sh -ros2 run obstacle_avoidance_planner calculation_time_plotter.py +ros2 run autoware_path_optimizer calculation_time_plotter.py ``` ![calculation_time_plot](../media/debug/calculation_time_plot.png) @@ -143,7 +143,7 @@ ros2 run obstacle_avoidance_planner calculation_time_plotter.py You can specify functions to plot with the `-f` option. ```sh -ros2 run obstacle_avoidance_planner calculation_time_plotter.py -f "onPath, generateOptimizedTrajectory, calcReferencePoints" +ros2 run autoware_path_optimizer calculation_time_plotter.py -f "onPath, generateOptimizedTrajectory, calcReferencePoints" ``` ## Q&A for Debug diff --git a/planning/obstacle_avoidance_planner/docs/mpt.md b/planning/autoware_path_optimizer/docs/mpt.md similarity index 100% rename from planning/obstacle_avoidance_planner/docs/mpt.md rename to planning/autoware_path_optimizer/docs/mpt.md diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp similarity index 91% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp index 4f4f01bf1ec9f..b27eff787ca5a 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ -#include "obstacle_avoidance_planner/type_alias.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -25,7 +25,7 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { struct ReferencePoint; struct Bounds; @@ -65,7 +65,7 @@ struct TimeKeeper latest_stream.str(""); if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("obstacle_avoidance_planner.time"), msg); + RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_optimizer.time"), msg); } } @@ -153,6 +153,6 @@ struct EgoNearestParam double dist_threshold{0.0}; double yaw_threshold{0.0}; }; -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer -#endif // OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp similarity index 75% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp index ce12cbca8ddb6..3ef47b865bf8e 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp @@ -11,11 +11,11 @@ // 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 OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "rclcpp/clock.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" @@ -25,11 +25,11 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker); -} // namespace obstacle_avoidance_planner -#endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ +} // namespace autoware_path_optimizer +#endif // AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp similarity index 96% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp index edcabec14930d..33da339c62e40 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/state_equation_generator.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/state_equation_generator.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -34,7 +34,7 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { struct Bounds { @@ -308,5 +308,5 @@ class MPTOptimizer size_t getNumberOfSlackVariables() const; std::optional calcNormalizedAvoidanceCost(const ReferencePoint & ref_point) const; }; -} // namespace obstacle_avoidance_planner -#endif // OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ +} // namespace autoware_path_optimizer +#endif // AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp similarity index 89% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp index db17496288766..a0e776628e3cb 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware_path_optimizer/replan_checker.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/replan_checker.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/polling_subscriber.hpp" @@ -33,12 +33,12 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { -class ObstacleAvoidancePlanner : public rclcpp::Node +class PathOptimizer : public rclcpp::Node { public: - explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); + explicit PathOptimizer(const rclcpp::NodeOptions & node_options); // NOTE: This is for the static_centerline_generator package which utilizes the following // instance. @@ -141,6 +141,6 @@ class ObstacleAvoidancePlanner : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer -#endif // OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp similarity index 83% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp index 1e4699556ef24..8270bb631e0ae 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { class ReplanChecker { @@ -66,6 +66,6 @@ class ReplanChecker bool isPathGoalChanged( const PlannerData & planner_data, const std::vector & prev_traj_points) const; }; -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer -#endif // OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp similarity index 76% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp index b6f4eadb682fd..6ee4fcb0ab7a5 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp" #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { struct ReferencePoint; @@ -58,5 +58,5 @@ class StateEquationGenerator std::unique_ptr vehicle_model_ptr_; mutable std::shared_ptr time_keeper_ptr_; }; -} // namespace obstacle_avoidance_planner -#endif // OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ +} // namespace autoware_path_optimizer +#endif // AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp similarity index 87% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp index b02d2232a4aa1..f249b7486bce6 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" @@ -28,7 +28,7 @@ #include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { // std_msgs using std_msgs::msg::Header; @@ -45,6 +45,6 @@ using visualization_msgs::msg::MarkerArray; // debug using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer -#endif // OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp similarity index 77% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp index 1ec61f583f661..d21d353f951c4 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -39,13 +39,13 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p); +geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p); +geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p); } // namespace tier4_autoware_utils -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace geometry_utils { @@ -68,5 +68,5 @@ bool isOutsideDrivableAreaFromRectangleFootprint( const vehicle_info_util::VehicleInfo & vehicle_info, const bool use_footprint_polygon_for_outside_drivable_area_check); } // namespace geometry_utils -} // namespace obstacle_avoidance_planner -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ +} // namespace autoware_path_optimizer +#endif // AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp similarity index 89% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp index d119acf09b6ea..c4d3f9c063433 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" #include @@ -38,16 +38,16 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p); +geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p); +geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p); template <> -double getLongitudinalVelocity(const obstacle_avoidance_planner::ReferencePoint & p); +double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p); } // namespace tier4_autoware_utils -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace trajectory_utils { @@ -181,7 +181,7 @@ std::optional updateFrontPointForFix( motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_DEBUG( - rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + rclcpp::get_logger("autoware_path_optimizer.trajectory_utils"), "Fixed point will not be inserted due to the error during calculation."); return std::nullopt; } @@ -192,7 +192,7 @@ std::optional updateFrontPointForFix( constexpr double max_lat_error = 3.0; if (max_lat_error < dist) { RCLCPP_DEBUG( - rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + rclcpp::get_logger("autoware_path_optimizer.trajectory_utils"), "New Fixed point is too far from points %f [m]", dist); } @@ -214,5 +214,5 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx); } // namespace trajectory_utils -} // namespace obstacle_avoidance_planner -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ +} // namespace autoware_path_optimizer +#endif // AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp similarity index 76% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp index f892b0b8d13de..ec46a96e3d12b 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -32,4 +32,4 @@ class KinematicsBicycleModel : public VehicleModelInterface Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double curvature, const double ds) const override; }; -#endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp similarity index 84% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp index 3fd0129315edd..85180bd33b34c 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #include #include @@ -44,4 +44,4 @@ class VehicleModelInterface const double ds) const = 0; }; -#endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/planning/obstacle_avoidance_planner/launch/launch_visualization.launch.xml b/planning/autoware_path_optimizer/launch/launch_visualization.launch.xml similarity index 86% rename from planning/obstacle_avoidance_planner/launch/launch_visualization.launch.xml rename to planning/autoware_path_optimizer/launch/launch_visualization.launch.xml index d8320e09cc001..38b3d83e94991 100644 --- a/planning/obstacle_avoidance_planner/launch/launch_visualization.launch.xml +++ b/planning/autoware_path_optimizer/launch/launch_visualization.launch.xml @@ -13,6 +13,6 @@ - + diff --git a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml b/planning/autoware_path_optimizer/launch/path_optimizer.launch.xml similarity index 65% rename from planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml rename to planning/autoware_path_optimizer/launch/path_optimizer.launch.xml index aec23bf3e60ad..273e3fecb377d 100644 --- a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml +++ b/planning/autoware_path_optimizer/launch/path_optimizer.launch.xml @@ -2,8 +2,8 @@ - - + + diff --git a/planning/obstacle_avoidance_planner/media/avoid_sudden_steering.drawio.svg b/planning/autoware_path_optimizer/media/avoid_sudden_steering.drawio.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/avoid_sudden_steering.drawio.svg rename to planning/autoware_path_optimizer/media/avoid_sudden_steering.drawio.svg diff --git a/planning/obstacle_avoidance_planner/media/avoidance_cost.drawio.svg b/planning/autoware_path_optimizer/media/avoidance_cost.drawio.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/avoidance_cost.drawio.svg rename to planning/autoware_path_optimizer/media/avoidance_cost.drawio.svg diff --git a/planning/obstacle_avoidance_planner/media/bounds.svg b/planning/autoware_path_optimizer/media/bounds.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/bounds.svg rename to planning/autoware_path_optimizer/media/bounds.svg diff --git a/planning/obstacle_avoidance_planner/media/debug/bounds_visualization.png b/planning/autoware_path_optimizer/media/debug/bounds_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/bounds_visualization.png rename to planning/autoware_path_optimizer/media/debug/bounds_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/calculation_time_plot.png b/planning/autoware_path_optimizer/media/debug/calculation_time_plot.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/calculation_time_plot.png rename to planning/autoware_path_optimizer/media/debug/calculation_time_plot.png diff --git a/planning/obstacle_avoidance_planner/media/debug/drivable_area_visualization.png b/planning/autoware_path_optimizer/media/debug/drivable_area_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/drivable_area_visualization.png rename to planning/autoware_path_optimizer/media/debug/drivable_area_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_fixed_traj_visualization.png b/planning/autoware_path_optimizer/media/debug/mpt_fixed_traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/mpt_fixed_traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/mpt_fixed_traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_ref_traj_visualization.png b/planning/autoware_path_optimizer/media/debug/mpt_ref_traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/mpt_ref_traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/mpt_ref_traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_traj_visualization.png b/planning/autoware_path_optimizer/media/debug/mpt_traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/mpt_traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/mpt_traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/path_footprint_visualization.png b/planning/autoware_path_optimizer/media/debug/path_footprint_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/path_footprint_visualization.png rename to planning/autoware_path_optimizer/media/debug/path_footprint_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/path_visualization.png b/planning/autoware_path_optimizer/media/debug/path_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/path_visualization.png rename to planning/autoware_path_optimizer/media/debug/path_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/traj_footprint_visualization.png b/planning/autoware_path_optimizer/media/debug/traj_footprint_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/traj_footprint_visualization.png rename to planning/autoware_path_optimizer/media/debug/traj_footprint_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/traj_visualization.png b/planning/autoware_path_optimizer/media/debug/traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/vehicle_circles_visualization.png b/planning/autoware_path_optimizer/media/debug/vehicle_circles_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/vehicle_circles_visualization.png rename to planning/autoware_path_optimizer/media/debug/vehicle_circles_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/vehicle_traj_circles_visualization.png b/planning/autoware_path_optimizer/media/debug/vehicle_traj_circles_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/vehicle_traj_circles_visualization.png rename to planning/autoware_path_optimizer/media/debug/vehicle_traj_circles_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/keep_minimum_boundary_width.drawio.svg b/planning/autoware_path_optimizer/media/keep_minimum_boundary_width.drawio.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/keep_minimum_boundary_width.drawio.svg rename to planning/autoware_path_optimizer/media/keep_minimum_boundary_width.drawio.svg diff --git a/planning/obstacle_avoidance_planner/media/mpt_optimization_offset.svg b/planning/autoware_path_optimizer/media/mpt_optimization_offset.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/mpt_optimization_offset.svg rename to planning/autoware_path_optimizer/media/mpt_optimization_offset.svg diff --git a/planning/obstacle_avoidance_planner/media/vehicle_circles.svg b/planning/autoware_path_optimizer/media/vehicle_circles.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/vehicle_circles.svg rename to planning/autoware_path_optimizer/media/vehicle_circles.svg diff --git a/planning/obstacle_avoidance_planner/media/vehicle_error_kinematics.png b/planning/autoware_path_optimizer/media/vehicle_error_kinematics.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/vehicle_error_kinematics.png rename to planning/autoware_path_optimizer/media/vehicle_error_kinematics.png diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/autoware_path_optimizer/package.xml similarity index 93% rename from planning/obstacle_avoidance_planner/package.xml rename to planning/autoware_path_optimizer/package.xml index 40caf7ef300aa..9b2d1e952d91a 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -1,9 +1,9 @@ - obstacle_avoidance_planner + autoware_path_optimizer 0.1.0 - The obstacle_avoidance_planner package + The autoware_path_optimizer package Takayuki Murooka Kosuke Takeuchi Takamasa Horibe diff --git a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz b/planning/autoware_path_optimizer/rviz/autoware_path_optimizer.rviz similarity index 99% rename from planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz rename to planning/autoware_path_optimizer/rviz/autoware_path_optimizer.rviz index 78df4fe53c4d8..4d4feb38f27a6 100644 --- a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz +++ b/planning/autoware_path_optimizer/rviz/autoware_path_optimizer.rviz @@ -1038,7 +1038,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/eb_fixed_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/eb_fixed_traj Value: false View Footprint: Alpha: 1 @@ -1079,7 +1079,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/eb_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/eb_traj Value: false View Footprint: Alpha: 1 @@ -1124,7 +1124,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_ref_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/mpt_ref_traj Value: false View Footprint: Alpha: 1 @@ -1165,7 +1165,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_fixed_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/mpt_fixed_traj Value: false View Footprint: Alpha: 1 @@ -1206,7 +1206,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/marker Value: false - Class: rviz_plugins/Trajectory Color Border Vel Max: 3 @@ -1218,7 +1218,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/mpt_traj Value: false View Footprint: Alpha: 1 @@ -1263,7 +1263,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/trajectory Value: true View Footprint: Alpha: 1 diff --git a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py b/planning/autoware_path_optimizer/scripts/calculation_time_plotter.py similarity index 98% rename from planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py rename to planning/autoware_path_optimizer/scripts/calculation_time_plotter.py index 1904b45fb8225..29dfccb533fbe 100755 --- a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py +++ b/planning/autoware_path_optimizer/scripts/calculation_time_plotter.py @@ -33,7 +33,7 @@ def __init__(self, args): self.calculation_cost_hist = [] self.sub_calculation_cost = self.create_subscription( StringStamped, - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/calculation_time", + "/planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/calculation_time", self.CallbackCalculationCost, 1, ) diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp similarity index 98% rename from planning/obstacle_avoidance_planner/src/debug_marker.cpp rename to planning/autoware_path_optimizer/src/debug_marker.cpp index 2f8babf103877..3127d521160c2 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/debug_marker.hpp" +#include "autoware_path_optimizer/debug_marker.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -432,4 +432,4 @@ MarkerArray getDebugMarker( return marker_array; } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp similarity index 99% rename from planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp rename to planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 94dc62b1335d4..ece301e64c97e 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tf2/utils.h" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/normalization.hpp" @@ -29,7 +29,7 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace { @@ -1783,4 +1783,4 @@ std::optional MPTOptimizer::calcNormalizedAvoidanceCost( } return std::clamp(-negative_avoidance_cost / mpt_param_.max_avoidance_cost, 0.0, 1.0); } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp similarity index 93% rename from planning/obstacle_avoidance_planner/src/node.cpp rename to planning/autoware_path_optimizer/src/node.cpp index b05d3f9da0c57..49d41e6b07884 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/node.hpp" +#include "autoware_path_optimizer/node.hpp" +#include "autoware_path_optimizer/debug_marker.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "obstacle_avoidance_planner/debug_marker.hpp" -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace { @@ -82,8 +82,8 @@ std::vector calcSegmentLengthVector(const std::vector & } } // namespace -ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options) -: Node("obstacle_avoidance_planner", node_options), +PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) +: Node("path_optimizer", node_options), vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), debug_data_ptr_(std::make_shared()), time_keeper_ptr_(std::make_shared()) @@ -94,7 +94,7 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // interface subscriber path_sub_ = create_subscription( - "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); + "~/input/path", 1, std::bind(&PathOptimizer::onPath, this, std::placeholders::_1)); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -148,13 +148,13 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been // initialized. set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); + std::bind(&PathOptimizer::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); published_time_publisher_ = std::make_unique(this); } -rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( +rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( const std::vector & parameters) { using tier4_autoware_utils::updateParam; @@ -203,7 +203,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( return result; } -void ObstacleAvoidancePlanner::initializePlanning() +void PathOptimizer::initializePlanning() { RCLCPP_DEBUG(get_logger(), "Initialize planning"); @@ -212,12 +212,12 @@ void ObstacleAvoidancePlanner::initializePlanning() resetPreviousData(); } -void ObstacleAvoidancePlanner::resetPreviousData() +void PathOptimizer::resetPreviousData() { mpt_optimizer_ptr_->resetPreviousData(); } -void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) +void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); @@ -282,7 +282,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); } -bool ObstacleAvoidancePlanner::checkInputPath(const Path & path, rclcpp::Clock clock) const +bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const { if (path.points.size() < 2) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Path points size is less than 1."); @@ -298,7 +298,7 @@ bool ObstacleAvoidancePlanner::checkInputPath(const Path & path, rclcpp::Clock c return true; } -PlannerData ObstacleAvoidancePlanner::createPlannerData( +PlannerData PathOptimizer::createPlannerData( const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const { // create planner data @@ -314,7 +314,7 @@ PlannerData ObstacleAvoidancePlanner::createPlannerData( return planner_data; } -std::vector ObstacleAvoidancePlanner::generateOptimizedTrajectory( +std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); @@ -341,8 +341,7 @@ std::vector ObstacleAvoidancePlanner::generateOptimizedTrajecto return optimized_traj_points; } -std::vector ObstacleAvoidancePlanner::optimizeTrajectory( - const PlannerData & planner_data) +std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); const auto & p = planner_data; @@ -375,7 +374,7 @@ std::vector ObstacleAvoidancePlanner::optimizeTrajectory( return mpt_traj; } -std::vector ObstacleAvoidancePlanner::getPrevOptimizedTrajectory( +std::vector PathOptimizer::getPrevOptimizedTrajectory( const std::vector & traj_points) const { const auto prev_optimized_traj_points = mpt_optimizer_ptr_->getPrevOptimizedTrajectoryPoints(); @@ -385,7 +384,7 @@ std::vector ObstacleAvoidancePlanner::getPrevOptimizedTrajector return traj_points; } -void ObstacleAvoidancePlanner::applyInputVelocity( +void PathOptimizer::applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const @@ -488,7 +487,7 @@ void ObstacleAvoidancePlanner::applyInputVelocity( time_keeper_ptr_->toc(__func__, " "); } -void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( +void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { time_keeper_ptr_->tic(__func__); @@ -555,7 +554,7 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( time_keeper_ptr_->toc(__func__, " "); } -void ObstacleAvoidancePlanner::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const +void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { time_keeper_ptr_->tic(__func__); @@ -570,7 +569,7 @@ void ObstacleAvoidancePlanner::publishVirtualWall(const geometry_msgs::msg::Pose time_keeper_ptr_->toc(__func__, " "); } -void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization( +void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { if (!enable_pub_debug_marker_) { @@ -592,7 +591,7 @@ void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization( time_keeper_ptr_->toc(__func__, " "); } -std::vector ObstacleAvoidancePlanner::extendTrajectory( +std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { @@ -656,7 +655,7 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( return resampled_traj_points; } -void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const +void PathOptimizer::publishDebugData(const Header & header) const { time_keeper_ptr_->tic(__func__); @@ -667,7 +666,7 @@ void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const time_keeper_ptr_->toc(__func__, " "); } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_avoidance_planner::ObstacleAvoidancePlanner) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware_path_optimizer::PathOptimizer) diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp similarity index 97% rename from planning/obstacle_avoidance_planner/src/replan_checker.cpp rename to planning/autoware_path_optimizer/src/replan_checker.cpp index 5f495051cc2bd..797041ee75416 100644 --- a/planning/obstacle_avoidance_planner/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/replan_checker.hpp" +#include "autoware_path_optimizer/replan_checker.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) : ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) @@ -212,4 +212,4 @@ bool ReplanChecker::isPathGoalChanged( return true; } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp similarity index 92% rename from planning/obstacle_avoidance_planner/src/state_equation_generator.cpp rename to planning/autoware_path_optimizer/src/state_equation_generator.cpp index 7460ce9c1f764..7712fbbf6c3cf 100644 --- a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/state_equation_generator.hpp" +#include "autoware_path_optimizer/state_equation_generator.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { // state equation: x = B u + W (u includes x_0) // NOTE: Originally, x_t+1 = Ad x_t + Bd u + Wd. @@ -69,4 +69,4 @@ Eigen::VectorXd StateEquationGenerator::predict( { return mat.B * U + mat.W; } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp similarity index 97% rename from planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp rename to planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 3f35de9147e6a..e05098d126a74 100644 --- a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "tf2/utils.h" #include @@ -36,7 +36,7 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace bg = boost::geometry; using tier4_autoware_utils::LinearRing2d; @@ -207,4 +207,4 @@ bool isOutsideDrivableAreaFromRectangleFootprint( return false; } } // namespace geometry_utils -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp similarity index 93% rename from planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp rename to planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 651b11b28e8bc..66d0fc08ccf06 100644 --- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "tf2/utils.h" #include "autoware_auto_planning_msgs/msg/path_point.hpp" @@ -36,25 +36,25 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p) +geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p) { return p.pose.position; } template <> -geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p) +geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p) { return p.pose; } template <> -double getLongitudinalVelocity(const obstacle_avoidance_planner::ReferencePoint & p) +double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p) { return p.longitudinal_velocity_mps; } } // namespace tier4_autoware_utils -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace trajectory_utils { @@ -242,4 +242,4 @@ void insertStopPoint( traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); } } // namespace trajectory_utils -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp similarity index 94% rename from planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp rename to planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 19ff1775a1211..89e9e0e2c9b74 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp similarity index 92% rename from planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp rename to planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp index 07a2ab00e7546..57e4671a4e0cc 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp +++ b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp" VehicleModelInterface::VehicleModelInterface( const int dim_x, const int dim_u, const int dim_y, const double wheelbase, diff --git a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp similarity index 70% rename from planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp rename to planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index d5af4c7e1180f..8ef099ba09f24 100644 --- a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/node.hpp" +#include "autoware_path_optimizer/node.hpp" #include #include @@ -32,27 +32,26 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto obstacle_avoidance_planner_dir = - ament_index_cpp::get_package_share_directory("obstacle_avoidance_planner"); + const auto path_optimizer_dir = + ament_index_cpp::get_package_share_directory("autoware_path_optimizer"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_common.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", - obstacle_avoidance_planner_dir + "/config/obstacle_avoidance_planner.param.yaml"}); + path_optimizer_dir + "/config/path_optimizer.param.yaml"}); - auto test_target_node = - std::make_shared(node_options); + auto test_target_node = std::make_shared(node_options); // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "obstacle_avoidance_planner/input/odometry"); + test_manager->publishOdometry(test_target_node, "path_optimizer/input/odometry"); - // set subscriber with topic name: obstacle_avoidance_planner → test_node_ - test_manager->setTrajectorySubscriber("obstacle_avoidance_planner/output/path"); + // set subscriber with topic name: path_optimizer → test_node_ + test_manager->setTrajectorySubscriber("path_optimizer/output/path"); - // set obstacle_avoidance_planner's input topic name(this topic is changed to test node) - test_manager->setPathInputTopicName("obstacle_avoidance_planner/input/path"); + // set path_optimizer's input topic name(this topic is changed to test node) + test_manager->setPathInputTopicName("path_optimizer/input/path"); // test with normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node)); diff --git a/planning/autoware_planning_test_manager/README.md b/planning/autoware_planning_test_manager/README.md index b72625be999dd..fe5484ca498ee 100644 --- a/planning/autoware_planning_test_manager/README.md +++ b/planning/autoware_planning_test_manager/README.md @@ -49,7 +49,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) // publish the necessary topics from test_manager second argument is topic name test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); test_manager->publishMaxVelocity( - test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); + test_target_node, "velocity_smoother/input/external_velocity_limit_mps"); // set scenario_selector's input topic name(this topic is changed to test node) test_manager->setTrajectoryInputTopicName("input/parking/trajectory"); @@ -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 | -| motion_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 | -| obstacle_avoidance_planner | 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 | +| ------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | +| 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/autoware_static_centerline_generator/README.md b/planning/autoware_static_centerline_generator/README.md index 844d0af15f2a5..426d5487cf0cb 100644 --- a/planning/autoware_static_centerline_generator/README.md +++ b/planning/autoware_static_centerline_generator/README.md @@ -5,7 +5,7 @@ This package statically calculates the centerline satisfying path footprints inside the drivable area. On narrow-road driving, the default centerline, which is the middle line between lanelets' right and left boundaries, often causes path footprints outside the drivable area. -To make path footprints inside the drivable area, we use online path shape optimization by [the obstacle_avoidance_planner package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/obstacle_avoidance_planner/). +To make path footprints inside the drivable area, we use online path shape optimization by [the autoware_path_optimizer package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/autoware_path_optimizer/). Instead of online path shape optimization, we introduce static centerline optimization. With this static centerline optimization, we have following advantages. diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml index 8e016e2b63391..486ea547e3dc1 100644 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -28,10 +28,7 @@ default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml" /> - + @@ -76,7 +73,7 @@ - + diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 56d30a715c006..62cdecc1f1eb2 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -18,6 +18,7 @@ autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_path_optimizer behavior_path_planner_common geography_utils geometry_msgs @@ -28,7 +29,6 @@ map_projection_loader mission_planner motion_utils - obstacle_avoidance_planner osqp_interface path_smoother rclcpp diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 2825381937674..9eb9a2b432a21 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -14,9 +14,9 @@ #include "centerline_source/optimization_trajectory_based_centerline.hpp" +#include "autoware_path_optimizer/node.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "obstacle_avoidance_planner/node.hpp" #include "path_smoother/elastic_band_smoother.hpp" #include "static_centerline_generator_node.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" @@ -130,7 +130,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra const auto eb_path_smoother_ptr = path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); const auto mpt_optimizer_ptr = - obstacle_avoidance_planner::ObstacleAvoidancePlanner(create_node_options()).getMPTOptimizer(); + autoware_path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); // NOTE: The optimization is executed every valid_optimized_traj_points_num points. constexpr int valid_optimized_traj_points_num = 10; @@ -156,9 +156,9 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra const auto smoothed_traj_points = eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); - // road collision avoidance by model predictive trajectory in the obstacle_avoidance_planner + // road collision avoidance by model predictive trajectory in the autoware_path_optimizer // package - const obstacle_avoidance_planner::PlannerData planner_data{ + const autoware_path_optimizer::PlannerData planner_data{ raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, virtual_ego_pose}; const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index dc3c26798b12d..7a5cdfd905fe9 100644 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -67,8 +67,8 @@ def generate_test_description(): "config/elastic_band_smoother.param.yaml", ), os.path.join( - get_package_share_directory("obstacle_avoidance_planner"), - "config/obstacle_avoidance_planner.param.yaml", + get_package_share_directory("autoware_path_optimizer"), + "config/path_optimizer.param.yaml", ), os.path.join( get_package_share_directory("map_loader"), diff --git a/planning/motion_velocity_smoother/CMakeLists.txt b/planning/autoware_velocity_smoother/CMakeLists.txt similarity index 72% rename from planning/motion_velocity_smoother/CMakeLists.txt rename to planning/autoware_velocity_smoother/CMakeLists.txt index 7fd9c5681315c..fee13513a7a97 100644 --- a/planning/motion_velocity_smoother/CMakeLists.txt +++ b/planning/autoware_velocity_smoother/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(motion_velocity_smoother) +project(autoware_velocity_smoother) find_package(autoware_cmake REQUIRED) autoware_package() @@ -14,7 +14,7 @@ include_directories( ) set(MOTION_VELOCITY_SMOOTHER_SRC - src/motion_velocity_smoother_node.cpp + src/node.cpp ) set(SMOOTHER_SRC @@ -32,17 +32,17 @@ ament_auto_add_library(smoother SHARED ${SMOOTHER_SRC} ) -ament_auto_add_library(motion_velocity_smoother_node SHARED +ament_auto_add_library(${PROJECT_NAME}_node SHARED ${MOTION_VELOCITY_SMOOTHER_SRC} ) -target_link_libraries(motion_velocity_smoother_node +target_link_libraries(${PROJECT_NAME}_node smoother ) -rclcpp_components_register_node(motion_velocity_smoother_node - PLUGIN "motion_velocity_smoother::MotionVelocitySmootherNode" - EXECUTABLE motion_velocity_smoother +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "autoware_velocity_smoother::VelocitySmootherNode" + EXECUTABLE velocity_smoother_node ) if(BUILD_TESTING) @@ -53,10 +53,10 @@ if(BUILD_TESTING) smoother ) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_motion_velocity_smoother_node_interface.cpp + test/test_velocity_smoother_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} - motion_velocity_smoother_node + ${PROJECT_NAME}_node ) endif() diff --git a/planning/motion_velocity_smoother/README.ja.md b/planning/autoware_velocity_smoother/README.ja.md similarity index 98% rename from planning/motion_velocity_smoother/README.ja.md rename to planning/autoware_velocity_smoother/README.ja.md index 62e97b6bbc906..68957078e2bdb 100644 --- a/planning/motion_velocity_smoother/README.ja.md +++ b/planning/autoware_velocity_smoother/README.ja.md @@ -1,10 +1,10 @@ -# Motion Velocity Smoother +# Velocity Smoother ## Purpose -`motion_velocity_smoother`は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 +`autoware_velocity_smoother`は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 -加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールを`motion_velocity_smoother`と呼んでいる。 +加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールを`autoware_velocity_smoother`と呼んでいる。 ## Inner-workings / Algorithms diff --git a/planning/motion_velocity_smoother/README.md b/planning/autoware_velocity_smoother/README.md similarity index 98% rename from planning/motion_velocity_smoother/README.md rename to planning/autoware_velocity_smoother/README.md index 1ff7f5982b4eb..b8a8a9eb7bb5f 100644 --- a/planning/motion_velocity_smoother/README.md +++ b/planning/autoware_velocity_smoother/README.md @@ -1,10 +1,10 @@ -# Motion Velocity Smoother +# Velocity Smoother ## Purpose -`motion_velocity_smoother` outputs a desired velocity profile on a reference trajectory. +`autoware_velocity_smoother` outputs a desired velocity profile on a reference trajectory. This module plans a velocity profile within the limitations of the velocity, the acceleration and the jerk to realize both the maximization of velocity and the ride quality. -We call this module `motion_velocity_smoother` because the limitations of the acceleration and the jerk means the smoothness of the velocity profile. +We call this module `autoware_velocity_smoother` because the limitations of the acceleration and the jerk means the smoothness of the velocity profile. ## Inner-workings / Algorithms @@ -18,7 +18,7 @@ For the point on the reference trajectory closest to the center of the rear whee #### Apply external velocity limit -It applies the velocity limit input from the external of `motion_velocity_smoother`. +It applies the velocity limit input from the external of `autoware_velocity_smoother`. Remark that the external velocity limit is different from the velocity limit already set on the map and the reference trajectory. The external velocity is applied at the position that it is able to reach the velocity limit with the deceleration and the jerk constraints set as the parameter. diff --git a/planning/motion_velocity_smoother/config/Analytical.param.yaml b/planning/autoware_velocity_smoother/config/Analytical.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/Analytical.param.yaml rename to planning/autoware_velocity_smoother/config/Analytical.param.yaml diff --git a/planning/motion_velocity_smoother/config/JerkFiltered.param.yaml b/planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/JerkFiltered.param.yaml rename to planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml diff --git a/planning/motion_velocity_smoother/config/L2.param.yaml b/planning/autoware_velocity_smoother/config/L2.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/L2.param.yaml rename to planning/autoware_velocity_smoother/config/L2.param.yaml diff --git a/planning/motion_velocity_smoother/config/Linf.param.yaml b/planning/autoware_velocity_smoother/config/Linf.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/Linf.param.yaml rename to planning/autoware_velocity_smoother/config/Linf.param.yaml diff --git a/planning/motion_velocity_smoother/config/default_common.param.yaml b/planning/autoware_velocity_smoother/config/default_common.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/default_common.param.yaml rename to planning/autoware_velocity_smoother/config/default_common.param.yaml diff --git a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml b/planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml rename to planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml diff --git a/planning/motion_velocity_smoother/config/rqt_multiplot.xml b/planning/autoware_velocity_smoother/config/rqt_multiplot.xml similarity index 96% rename from planning/motion_velocity_smoother/config/rqt_multiplot.xml rename to planning/autoware_velocity_smoother/config/rqt_multiplot.xml index ad0ff4b68251e..694f18c0d9b4f 100644 --- a/planning/motion_velocity_smoother/config/rqt_multiplot.xml +++ b/planning/autoware_velocity_smoother/config/rqt_multiplot.xml @@ -35,7 +35,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_velocity + /planning/scenario_planning/velocity_smoother/closest_velocity std_msgs/Float32 @@ -48,7 +48,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_velocity + /planning/scenario_planning/velocity_smoother/closest_velocity std_msgs/Float32 @@ -86,7 +86,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_merged_velocity + /planning/scenario_planning/velocity_smoother/closest_merged_velocity std_msgs/Float32 @@ -99,7 +99,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_merged_velocity + /planning/scenario_planning/velocity_smoother/closest_merged_velocity std_msgs/Float32 @@ -137,7 +137,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_max_velocity + /planning/scenario_planning/velocity_smoother/closest_max_velocity std_msgs/Float32 @@ -150,7 +150,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_max_velocity + /planning/scenario_planning/velocity_smoother/closest_max_velocity std_msgs/Float32 @@ -188,7 +188,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_acceleration + /planning/scenario_planning/velocity_smoother/closest_acceleration std_msgs/Float32 @@ -201,7 +201,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_acceleration + /planning/scenario_planning/velocity_smoother/closest_acceleration std_msgs/Float32 @@ -264,7 +264,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/calculation_time + /planning/scenario_planning/velocity_smoother/calculation_time std_msgs/Float32 @@ -277,7 +277,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/calculation_time + /planning/scenario_planning/velocity_smoother/calculation_time std_msgs/Float32 diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp similarity index 92% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp index 868c1ab12cce8..c2fe415091a0a 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ -#define MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ - +#ifndef AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ + +#include "autoware_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/resample.hpp" -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" -#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -53,7 +53,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -77,10 +77,10 @@ struct Motion Motion(const double v, const double a) : vel(v), acc(a) {} }; -class MotionVelocitySmootherNode : public rclcpp::Node +class VelocitySmootherNode : public rclcpp::Node { public: - explicit MotionVelocitySmootherNode(const rclcpp::NodeOptions & node_options); + explicit VelocitySmootherNode(const rclcpp::NodeOptions & node_options); private: rclcpp::Publisher::SharedPtr pub_trajectory_; @@ -275,6 +275,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp similarity index 90% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp index 80f691b5f7b40..4c820b4d04baa 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ -#define MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace resampling { @@ -48,6 +48,6 @@ TrajectoryPoints resampleTrajectory( const double nearest_dist_threshold, const double nearest_yaw_threshold, const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true); } // namespace resampling -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp similarity index 85% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 9cddba5ca500a..08d0a1cd1a8e1 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -28,7 +28,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { class AnalyticalJerkConstrainedSmoother : public SmootherBase { @@ -113,8 +113,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase std::string strTimes(const std::vector & times) const; std::string strStartIndices(const std::vector & start_indices) const; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother // clang-format off -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT // clang-format on diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp similarity index 82% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 554bdcca890ef..596a8aa94c707 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" @@ -25,7 +25,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace analytical_velocity_planning_utils { @@ -51,8 +51,8 @@ double integ_x(double x0, double v0, double a0, double j0, double t); double integ_v(double v0, double a0, double j0, double t); double integ_a(double a0, double j0, double t); } // namespace analytical_velocity_planning_utils -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother // clang-format off -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT // clang-format on diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp similarity index 86% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 4b55fbbf184b9..57414b8e8109f 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -26,7 +26,7 @@ #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { class JerkFilteredSmoother : public SmootherBase { @@ -69,6 +69,6 @@ class JerkFilteredSmoother : public SmootherBase const double v0, const double a0, const double a_min, const double j_min, const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp similarity index 82% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 392fef2429736..3d94102495839 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -26,7 +26,7 @@ #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { class L2PseudoJerkSmoother : public SmootherBase { @@ -56,6 +56,6 @@ class L2PseudoJerkSmoother : public SmootherBase autoware::common::osqp::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")}; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp similarity index 82% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 9cc6ffef16090..c8bdc9d3c939c 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -26,7 +26,7 @@ #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { class LinfPseudoJerkSmoother : public SmootherBase { @@ -56,6 +56,6 @@ class LinfPseudoJerkSmoother : public SmootherBase autoware::common::osqp::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")}; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp similarity index 91% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp index beb571635f70c..63d5e5e887124 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#include "motion_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/resample.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -23,7 +23,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -86,6 +86,6 @@ class SmootherBase protected: BaseParam base_param_; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp similarity index 91% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp index 35e8ab53dd75d..775f4109a4ad1 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ #include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include "geometry_msgs/msg/detail/pose__struct.hpp" @@ -23,7 +23,7 @@ #include #include -namespace motion_velocity_smoother::trajectory_utils +namespace autoware_velocity_smoother::trajectory_utils { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -77,6 +77,6 @@ std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest); -} // namespace motion_velocity_smoother::trajectory_utils +} // namespace autoware_velocity_smoother::trajectory_utils -#endif // MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml b/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml similarity index 77% rename from planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml rename to planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml index 86f767560fd7c..11932ad125adc 100644 --- a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml +++ b/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml @@ -1,6 +1,6 @@ - + @@ -12,10 +12,10 @@ - - + + - + diff --git a/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg b/planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg similarity index 100% rename from planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg rename to planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg diff --git a/planning/motion_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml similarity index 93% rename from planning/motion_velocity_smoother/package.xml rename to planning/autoware_velocity_smoother/package.xml index b9b368d917535..b390dcc12edc5 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -1,9 +1,9 @@ - motion_velocity_smoother + autoware_velocity_smoother 0.1.0 - The motion_velocity_smoother package + The autoware_velocity_smoother package Fumiya Watanabe Takamasa Horibe diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/autoware_velocity_smoother/src/node.cpp similarity index 91% rename from planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp rename to planning/autoware_velocity_smoother/src/node.cpp index eb8592bb99637..0ea10534eb126 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/motion_velocity_smoother_node.hpp" +#include "autoware_velocity_smoother/node.hpp" +#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "motion_utils/marker/marker_helper.hpp" -#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -32,10 +32,10 @@ #include // clang-format on -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { -MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions & node_options) -: Node("motion_velocity_smoother", node_options) +VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) +: Node("velocity_smoother", node_options) { using std::placeholders::_1; @@ -57,13 +57,13 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); pub_over_stop_velocity_ = create_publisher("~/stop_speed_exceeded", 1); sub_current_trajectory_ = create_subscription( - "~/input/trajectory", 1, std::bind(&MotionVelocitySmootherNode::onCurrentTrajectory, this, _1)); + "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); sub_current_odometry_ = create_subscription( "/localization/kinematic_state", 1, - std::bind(&MotionVelocitySmootherNode::onCurrentOdometry, this, _1)); + std::bind(&VelocitySmootherNode::onCurrentOdometry, this, _1)); sub_external_velocity_limit_ = create_subscription( "~/input/external_velocity_limit_mps", 1, - std::bind(&MotionVelocitySmootherNode::onExternalVelocityLimit, this, _1)); + std::bind(&VelocitySmootherNode::onExternalVelocityLimit, this, _1)); sub_current_acceleration_ = create_subscription( "~/input/acceleration", 1, [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { current_acceleration_ptr_ = msg; @@ -73,8 +73,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; }); // parameter update - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&MotionVelocitySmootherNode::onParameter, this, _1)); + set_param_res_ = + this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1)); // debug publish_debug_trajs_ = declare_parameter("publish_debug_trajs"); @@ -107,7 +107,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions published_time_publisher_ = std::make_unique(this); } -void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) +void VelocitySmootherNode::setupSmoother(const double wheelbase) { switch (node_param_.algorithm_type) { case AlgorithmType::JERK_FILTERED: { @@ -137,13 +137,13 @@ void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) break; } default: - throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); + throw std::domain_error("[VelocitySmootherNode] invalid algorithm"); } smoother_->setWheelBase(wheelbase); } -rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter( +rcl_interfaces::msg::SetParametersResult VelocitySmootherNode::onParameter( const std::vector & parameters) { auto update_param = [&](const std::string & name, double & v) { @@ -263,7 +263,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter break; } default: - throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); + throw std::domain_error("[VelocitySmootherNode] invalid algorithm"); } rcl_interfaces::msg::SetParametersResult result{}; @@ -272,7 +272,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter return result; } -void MotionVelocitySmootherNode::initCommonParam() +void VelocitySmootherNode::initCommonParam() { auto & p = node_param_; p.enable_lateral_acc_limit = declare_parameter("enable_lateral_acc_limit"); @@ -310,7 +310,7 @@ void MotionVelocitySmootherNode::initCommonParam() declare_parameter("plan_from_ego_speed_on_manual_mode"); } -void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const +void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const { Trajectory publishing_trajectory = motion_utils::convertToTrajectory(trajectory); publishing_trajectory.header = base_traj_raw_ptr_->header; @@ -319,17 +319,17 @@ void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & traj pub_trajectory_, publishing_trajectory.header.stamp); } -void MotionVelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) +void VelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) { current_odometry_ptr_ = msg; } -void MotionVelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) +void VelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) { external_velocity_limit_ptr_ = msg; } -void MotionVelocitySmootherNode::calcExternalVelocityLimit() +void VelocitySmootherNode::calcExternalVelocityLimit() { if (!external_velocity_limit_ptr_) { return; @@ -418,7 +418,7 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit() return; } -bool MotionVelocitySmootherNode::checkData() const +bool VelocitySmootherNode::checkData() const { if (!current_odometry_ptr_ || !base_traj_raw_ptr_ || !current_acceleration_ptr_) { RCLCPP_DEBUG( @@ -434,7 +434,7 @@ bool MotionVelocitySmootherNode::checkData() const return true; } -void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) +void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) { RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); @@ -516,7 +516,7 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n"); } -void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit() +void VelocitySmootherNode::updateDataForExternalVelocityLimit() { if (prev_output_.empty()) { return; @@ -532,7 +532,7 @@ void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit() return; } -TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( +TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity( const TrajectoryPoints & traj_input) const { TrajectoryPoints output{}; // velocity is optimized by qp solver @@ -578,7 +578,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( return output; } -bool MotionVelocitySmootherNode::smoothVelocity( +bool VelocitySmootherNode::smoothVelocity( const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed) const { @@ -685,7 +685,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( return true; } -void MotionVelocitySmootherNode::insertBehindVelocity( +void VelocitySmootherNode::insertBehindVelocity( const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const { const bool keep_closest_vel_for_behind = @@ -727,7 +727,7 @@ void MotionVelocitySmootherNode::insertBehindVelocity( } } -void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const +void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const { const size_t closest = findNearestIndexFromEgo(trajectory); @@ -746,8 +746,7 @@ void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & tr pub_dist_to_stopline_->publish(dist_to_stopline); } -std::pair -MotionVelocitySmootherNode::calcInitialMotion( +std::pair VelocitySmootherNode::calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest) const { const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); @@ -830,7 +829,7 @@ MotionVelocitySmootherNode::calcInitialMotion( return {initial_motion, InitializeType::NORMAL}; } -void MotionVelocitySmootherNode::overwriteStopPoint( +void VelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { const auto stop_idx = motion_utils::searchZeroVelocityIndex(input); @@ -877,7 +876,7 @@ void MotionVelocitySmootherNode::overwriteStopPoint( } } -void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const +void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const { if (traj.size() < 1) { return; @@ -915,7 +914,7 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_.velocity); } -void MotionVelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const +void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { @@ -933,7 +932,7 @@ void MotionVelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & } } -void MotionVelocitySmootherNode::publishDebugTrajectories( +void VelocitySmootherNode::publishDebugTrajectories( const std::vector & debug_trajectories) const { auto debug_trajectories_tmp = debug_trajectories; @@ -955,7 +954,7 @@ void MotionVelocitySmootherNode::publishDebugTrajectories( } } -void MotionVelocitySmootherNode::publishClosestVelocity( +void VelocitySmootherNode::publishClosestVelocity( const TrajectoryPoints & trajectory, const Pose & current_pose, const rclcpp::Publisher::SharedPtr pub) const { @@ -967,7 +966,7 @@ void MotionVelocitySmootherNode::publishClosestVelocity( pub->publish(vel_data); } -void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) +void VelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) { const auto closest_point = calcProjectedTrajectoryPointFromEgo(trajectory); @@ -1002,13 +1001,13 @@ void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & tr *prev_time_ = curr_time; } -void MotionVelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result) +void VelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result) { prev_output_ = final_result; prev_closest_point_ = calcProjectedTrajectoryPointFromEgo(final_result); } -MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorithmType( +VelocitySmootherNode::AlgorithmType VelocitySmootherNode::getAlgorithmType( const std::string & algorithm_name) const { if (algorithm_name == "JerkFiltered") { @@ -1024,11 +1023,11 @@ MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorit return AlgorithmType::ANALYTICAL; } - throw std::domain_error("[MotionVelocitySmootherNode] undesired algorithm is selected."); + throw std::domain_error("[VelocitySmootherNode] undesired algorithm is selected."); return AlgorithmType::INVALID; } -double MotionVelocitySmootherNode::calcTravelDistance() const +double VelocitySmootherNode::calcTravelDistance() const { const auto closest_point = calcProjectedTrajectoryPointFromEgo(prev_output_); @@ -1041,14 +1040,14 @@ double MotionVelocitySmootherNode::calcTravelDistance() const return 0.0; } -bool MotionVelocitySmootherNode::isEngageStatus(const double target_vel) const +bool VelocitySmootherNode::isEngageStatus(const double target_vel) const { const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio; return vehicle_speed < engage_vel_thr && target_vel >= node_param_.engage_velocity; } -Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( +Trajectory VelocitySmootherNode::toTrajectoryMsg( const TrajectoryPoints & points, const std_msgs::msg::Header * header) const { auto trajectory = motion_utils::convertToTrajectory(points); @@ -1056,28 +1055,28 @@ Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( return trajectory; } -size_t MotionVelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const +size_t VelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const { return motion_utils::findFirstNearestIndexWithSoftConstraints( points, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); } -bool MotionVelocitySmootherNode::isReverse(const TrajectoryPoints & points) const +bool VelocitySmootherNode::isReverse(const TrajectoryPoints & points) const { if (points.empty()) return true; return std::any_of( points.begin(), points.end(), [](auto & pt) { return pt.longitudinal_velocity_mps < 0; }); } -void MotionVelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const +void VelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const { for (auto & pt : points) { pt.longitudinal_velocity_mps *= -1.0; } } -void MotionVelocitySmootherNode::publishStopWatchTime() +void VelocitySmootherNode::publishStopWatchTime() { Float32Stamped calculation_time_data{}; calculation_time_data.stamp = this->now(); @@ -1085,7 +1084,7 @@ void MotionVelocitySmootherNode::publishStopWatchTime() debug_calculation_time_->publish(calculation_time_data); } -TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPoint( +TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPoint( const TrajectoryPoints & trajectory, const Pose & pose) const { const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -1094,13 +1093,13 @@ TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPoint( return trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, pose, current_seg_idx); } -TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( +TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( const TrajectoryPoints & trajectory) const { return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(motion_velocity_smoother::MotionVelocitySmootherNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware_velocity_smoother::VelocitySmootherNode) diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp similarity index 98% rename from planning/motion_velocity_smoother/src/resample.cpp rename to planning/autoware_velocity_smoother/src/resample.cpp index ae638f8e4db1a..17ea1eb9fc9bd 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace resampling { @@ -264,4 +264,4 @@ TrajectoryPoints resampleTrajectory( } } // namespace resampling -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp similarity index 98% rename from planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 84d6331b3429d..d2956e67c6ac1 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include #include @@ -64,7 +64,7 @@ bool applyMaxVelocity( } // namespace -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node) : SmootherBase(node) @@ -658,4 +658,4 @@ std::string AnalyticalJerkConstrainedSmoother::strStartIndices( return ss.str(); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp similarity index 98% rename from planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp rename to planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 9fa4d697f06cb..84ac29d14bdc5 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace analytical_velocity_planning_utils { @@ -353,4 +353,4 @@ double integ_a(double a0, double j0, double t) } } // namespace analytical_velocity_planning_utils -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp similarity index 98% rename from planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 6c2bef3ae08c4..d212cfc0bcedf 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include @@ -27,7 +27,7 @@ #define VERBOSE_TRAJECTORY_VELOCITY false -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -322,7 +322,7 @@ bool JerkFilteredSmoother::apply( output.at(i).acceleration_mps2 = a_stop_decel; } - qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]"); + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); const int status_polish = std::get<2>(result); if (status_polish != 1) { @@ -480,4 +480,4 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( smoother_param_.jerk_filter_ds); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp similarity index 96% rename from planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index d46deeeeb9f10..3c2872f2e58e3 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include @@ -23,7 +23,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -223,7 +223,7 @@ bool L2PseudoJerkSmoother::apply( // v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); // } - qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]"); + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); const auto tf2 = std::chrono::system_clock::now(); const double dt_ms2 = @@ -242,4 +242,4 @@ TrajectoryPoints L2PseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp similarity index 96% rename from planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index e8900b1947454..f434d8d4514ca 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include @@ -23,7 +23,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -236,7 +236,7 @@ bool LinfPseudoJerkSmoother::apply( // v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); // } - qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]"); + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); const auto tf2 = std::chrono::system_clock::now(); const double dt_ms2 = @@ -254,4 +254,4 @@ TrajectoryPoints LinfPseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp similarity index 97% rename from planning/motion_velocity_smoother/src/smoother/smoother_base.cpp rename to planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index bf193b7251382..5704113067244 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/resample.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -26,7 +26,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace @@ -270,4 +270,4 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( return output; } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp similarity index 96% rename from planning/motion_velocity_smoother/src/trajectory_utils.cpp rename to planning/autoware_velocity_smoother/src/trajectory_utils.cpp index acc3673d66925..aff1a0b0e3213 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" @@ -25,7 +25,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { using geometry_msgs::msg::Point; namespace trajectory_utils @@ -202,7 +202,7 @@ std::vector calcTrajectoryCurvatureFrom3Points( } catch (std::exception const & e) { // ...code that handles the error... RCLCPP_WARN( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s", + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "%s", e.what()); if (i > 1) { curvature = k_arr.at(i - 1); // previous curvature @@ -374,7 +374,7 @@ bool calcStopDistWithJerkConstraints( double t1 = calculateTime(a0, 0.0, jerk); if (t1 < 0) { RCLCPP_WARN( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "t1 < 0. unexpected condition."); return false; } else if (t1 < t_threshold) { @@ -399,7 +399,7 @@ bool calcStopDistWithJerkConstraints( stop_dist = x; if (!isValidStopDist(v, a, target_vel, a_target, v_margin, a_margin)) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "valid error. type = " << static_cast(type)); return false; } @@ -416,13 +416,13 @@ bool isValidStopDist( const double a_max = a_target + std::abs(a_margin); if (v_end < v_min || v_max < v_end) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "valid check error! v_target = " << v_target << ", v_end = " << v_end); return false; } if (a_end < a_min || a_max < a_end) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "valid check error! a_target = " << a_target << ", a_end = " << a_end); return false; } @@ -486,7 +486,7 @@ std::optional applyDecelFilterWithJerkConstraint( const double a_margin{0.1}; if (!isValidStopDist(v, a, decel_target_vel, a_target, v_margin, a_margin)) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "validation check error"); return {}; } @@ -562,7 +562,7 @@ std::optional> updateStateWithJerkCon } RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "Invalid jerk profile"); return std::nullopt; } @@ -610,4 +610,4 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes } } // namespace trajectory_utils -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp similarity index 89% rename from planning/motion_velocity_smoother/test/test_smoother_functions.cpp rename to planning/autoware_velocity_smoother/test/test_smoother_functions.cpp index 67196edc17a2e..ace1d1f14d776 100644 --- a/planning/motion_velocity_smoother/test/test_smoother_functions.cpp +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include #include using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using motion_velocity_smoother::trajectory_utils::TrajectoryPoints; +using autoware_velocity_smoother::trajectory_utils::TrajectoryPoints; TrajectoryPoints genStraightTrajectory(const size_t size) { @@ -48,7 +48,7 @@ TEST(TestTrajectoryUtils, CalcTrajectoryCurvatureFrom3Points) const auto checkOutputSize = [](const size_t trajectory_size, const size_t idx_dist) { const auto trajectory_points = genStraightTrajectory(trajectory_size); const auto curvatures = - motion_velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( + autoware_velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( trajectory_points, idx_dist); EXPECT_EQ(curvatures.size(), trajectory_size) << ", idx_dist = " << idx_dist; }; diff --git a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp similarity index 74% rename from planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp rename to planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index 56813a37941a6..f0145ea5a32a7 100644 --- a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/motion_velocity_smoother_node.hpp" +#include "autoware_velocity_smoother/node.hpp" #include #include @@ -22,37 +22,36 @@ #include -using motion_velocity_smoother::MotionVelocitySmootherNode; +using autoware_velocity_smoother::VelocitySmootherNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); - test_manager->setTrajectorySubscriber("motion_velocity_smoother/output/trajectory"); - test_manager->setTrajectoryInputTopicName("motion_velocity_smoother/input/trajectory"); + test_manager->setTrajectorySubscriber("velocity_smoother/output/trajectory"); + test_manager->setTrajectoryInputTopicName("velocity_smoother/input/trajectory"); test_manager->setOdometryTopicName("/localization/kinematic_state"); return test_manager; } -std::shared_ptr generateNode() +std::shared_ptr generateNode() { auto node_options = rclcpp::NodeOptions{}; node_options.append_parameter_override("algorithm_type", "JerkFiltered"); node_options.append_parameter_override("publish_debug_trajs", false); const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto motion_velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_common.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - "--params-file", - motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", - "--params-file", motion_velocity_smoother_dir + "/config/default_common.param.yaml", - "--params-file", motion_velocity_smoother_dir + "/config/JerkFiltered.param.yaml"}); + "--params-file", velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + "--params-file", velocity_smoother_dir + "/config/default_common.param.yaml", "--params-file", + velocity_smoother_dir + "/config/JerkFiltered.param.yaml"}); - return std::make_shared(node_options); + return std::make_shared(node_options); } void publishMandatoryTopics( @@ -62,11 +61,10 @@ void publishMandatoryTopics( // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); test_manager->publishMaxVelocity( - test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); + test_target_node, "velocity_smoother/input/external_velocity_limit_mps"); test_manager->publishOperationModeState( - test_target_node, "motion_velocity_smoother/input/operation_mode_state"); - test_manager->publishAcceleration( - test_target_node, "motion_velocity_smoother/input/acceleration"); + test_target_node, "velocity_smoother/input/operation_mode_state"); + test_manager->publishAcceleration(test_target_node, "velocity_smoother/input/acceleration"); } TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index f7d204d2bff87..a12cadb0c4994 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -229,7 +229,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() // size of a drivable area. // The drivable area has to cover not the base link but the vehicle itself. Therefore // rear_overhang must be added to backward_path_length. In addition, because of the - // calculation of the drivable area in the obstacle_avoidance_planner package, the drivable + // calculation of the drivable area in the autoware_path_optimizer package, the drivable // area has to be a little longer than the backward_path_length parameter by adding // min_backward_offset. constexpr double min_backward_offset = 1.0; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 1511d430ddd0c..c85da1e551391 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -17,8 +17,8 @@ #include "route_handler/route_handler.hpp" +#include #include -#include #include #include @@ -88,7 +88,7 @@ struct PlannerData bool is_simulation = false; // velocity smoother - std::shared_ptr velocity_smoother_; + std::shared_ptr velocity_smoother_; // route handler std::shared_ptr route_handler_; // parameters diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 784b7cabfe9ad..ee3e33f6e2987 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -24,13 +24,13 @@ autoware_auto_planning_msgs autoware_auto_tf2 autoware_perception_msgs + autoware_velocity_smoother diagnostic_msgs eigen geometry_msgs interpolation lanelet2_extension motion_utils - motion_velocity_smoother nav_msgs objects_of_interest_marker_interface pcl_conversions diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 314281a954b49..725c717082620 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -15,9 +15,9 @@ // #include #include "motion_utils/trajectory/conversion.hpp" +#include #include #include -#include #include #include @@ -83,7 +83,7 @@ bool smoothPath( traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } out_path = motion_utils::convertToPathWithLaneId(traj_smoothed); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp index 84591f9429a66..c8d0ac54ca969 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ -#include +#include #include #include @@ -81,7 +81,7 @@ struct PlannerData tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; // velocity smoother - std::shared_ptr velocity_smoother_{}; + std::shared_ptr velocity_smoother_{}; // parameters vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index ad791d247ee11..b3eaaccd11d13 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -17,13 +17,13 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_velocity_smoother behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension libboost-dev motion_utils - motion_velocity_smoother rclcpp route_handler tier4_autoware_utils diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md index f8c5eb172abd2..9190adf1ff67e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -54,5 +54,5 @@ In addition, the following parameters should be provided to the node: - [nearest search parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/nearest_search.param.yaml); - [vehicle info parameters](https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml); - [common planning parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml); -- [smoother parameters](https://autowarefoundation.github.io/autoware.universe/main/planning/motion_velocity_smoother/#parameters) +- [smoother parameters](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_velocity_smoother/#parameters) - Parameters of each plugin that will be loaded. diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index 2b52610c0276a..0cbf220cd0955 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -21,13 +21,13 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_motion_velocity_planner_common + autoware_velocity_smoother diagnostic_msgs eigen geometry_msgs lanelet2_extension libboost-dev motion_utils - motion_velocity_smoother pcl_conversions pluginlib rclcpp diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index c0373678270dd..7c2c56c332865 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -14,10 +14,10 @@ #include "node.hpp" +#include +#include #include #include -#include -#include #include #include #include @@ -241,7 +241,7 @@ void MotionVelocityPlannerNode::on_acceleration( void MotionVelocityPlannerNode::set_velocity_smoother_params() { planner_data_.velocity_smoother_ = - std::make_shared(*this); + std::make_shared(*this); } void MotionVelocityPlannerNode::on_lanelet_map( @@ -397,7 +397,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } return traj_smoothed; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp index 3ad5bab8e070b..46887a56c84f2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp @@ -49,8 +49,8 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto motion_velocity_planner_dir = ament_index_cpp::get_package_share_directory("autoware_motion_velocity_planner_node"); - const auto motion_velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); const auto get_motion_velocity_module_config = [](const std::string & module) { const auto package_name = "autoware_motion_velocity_" + module + "_module"; @@ -67,14 +67,13 @@ std::shared_ptr generateNode() node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", - motion_velocity_smoother_dir + "/config/Analytical.param.yaml", - motion_velocity_planner_dir + "/config/motion_velocity_planner.param.yaml", - get_motion_velocity_module_config("out_of_lane")}); + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + motion_velocity_planner_dir + "/config/motion_velocity_planner.param.yaml", + get_motion_velocity_module_config("out_of_lane")}); return std::make_shared(node_options); } diff --git a/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml b/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml index 92b56c2d2e6e4..4c47fa994cc9b 100644 --- a/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml +++ b/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml @@ -8,10 +8,10 @@ - - - - + + + + @@ -69,10 +69,10 @@ - - - - + + + + diff --git a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py index c9255dae679c0..a54b238ad56e1 100755 --- a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py +++ b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py @@ -31,7 +31,7 @@ def __init__(self): self.sub_original_traj = self.create_subscription( Trajectory, - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory", + "/planning/scenario_planning/lane_driving/motion_planning/path_optimizer/trajectory", self.plotOriginalTrajectory, 1, ) diff --git a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml index fb64d2e0841bd..3f73e0f462cc2 100644 --- a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml +++ b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml @@ -1,6 +1,6 @@ - - + + diff --git a/planning/planning_validator/launch/planning_validator.launch.xml b/planning/planning_validator/launch/planning_validator.launch.xml index 382c2d22b2adf..81573adb4dc24 100644 --- a/planning/planning_validator/launch/planning_validator.launch.xml +++ b/planning/planning_validator/launch/planning_validator.launch.xml @@ -1,6 +1,6 @@ - + diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/path_sampler/README.md index 2d7fe52e5dcad..b82896f4fd93f 100644 --- a/planning/sampling_based_planner/path_sampler/README.md +++ b/planning/sampling_based_planner/path_sampler/README.md @@ -69,9 +69,9 @@ If the reference path is not smooth, the resulting candidates will probably be u Failure to find a valid trajectory current results in a suddenly stopping trajectory. -## Comparison with the `obstacle_avoidance_planner` +## Comparison with the `autoware_path_optimizer` -The `obstacle_avoidance_planner` uses an optimization based approach, +The `autoware_path_optimizer` uses an optimization based approach, finding the optimal solution of a mathematical problem if it exists. When no solution can be found, it is often hard to identify the issue due to the intermediate mathematical representation of the problem.