From d53e3b150c793ecdb4bd3d90c43e371076ac537a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 18 Jun 2024 18:38:05 +0900 Subject: [PATCH] refactor(motion_utils)!: add autoware prefix and include dir (#53) Signed-off-by: kosuke55 style(pre-commit): autofix --- .../tier4_debug_tools/lateral_error_publisher.hpp | 2 +- common/tier4_debug_tools/package.xml | 2 +- .../src/lateral_error_publisher.cpp | 2 +- .../config/logger_config.yaml | 12 ++++++------ driving_environment_analyzer/package.xml | 2 +- driving_environment_analyzer/src/utils.cpp | 10 +++++----- .../planning_debug_tools/trajectory_analyzer.hpp | 6 +++--- .../include/planning_debug_tools/util.hpp | 2 +- planning/planning_debug_tools/package.xml | 2 +- 9 files changed, 20 insertions(+), 20 deletions(-) diff --git a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp index 163f4995..6089aff7 100644 --- a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp +++ b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index 7c65a9ca..53454235 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -10,10 +10,10 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_planning_msgs autoware_universe_utils geometry_msgs - motion_utils rclcpp rclcpp_components tf2_ros diff --git a/common/tier4_debug_tools/src/lateral_error_publisher.cpp b/common/tier4_debug_tools/src/lateral_error_publisher.cpp index 13c87223..0989d994 100644 --- a/common/tier4_debug_tools/src/lateral_error_publisher.cpp +++ b/common/tier4_debug_tools/src/lateral_error_publisher.cpp @@ -74,7 +74,7 @@ void LateralErrorPublisher::onGroundTruthPose( } // Search closest trajectory point with vehicle pose - const auto closest_index = motion_utils::findNearestIndex( + const auto closest_index = autoware_motion_utils::findNearestIndex( current_trajectory_ptr_->points, current_vehicle_pose_ptr_->pose.pose, std::numeric_limits::max(), yaw_threshold_to_search_closest_); if (!closest_index) { diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index fef2a185..85d87eda 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -36,7 +36,7 @@ Planning: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: behavior_path_planner.path_shifter - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: motion_utils + logger_name: autoware_motion_utils behavior_path_planner_avoidance: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner @@ -74,7 +74,7 @@ Planning: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner - logger_name: motion_utils + logger_name: autoware_motion_utils behavior_velocity_planner_intersection: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner @@ -86,7 +86,7 @@ Planning: - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner - logger_name: motion_utils + logger_name: autoware_motion_utils motion_velocity_smoother: - node_name: /planning/scenario_planning/motion_velocity_smoother @@ -94,7 +94,7 @@ Planning: - node_name: /planning/scenario_planning/motion_velocity_smoother logger_name: autoware_universe_utils - node_name: /planning/scenario_planning/motion_velocity_smoother - logger_name: motion_utils + logger_name: autoware_motion_utils route_handler: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner @@ -110,7 +110,7 @@ Control: - node_name: /control/trajectory_follower/controller_node_exe logger_name: autoware_universe_utils - node_name: /control/trajectory_follower/controller_node_exe - logger_name: motion_utils + logger_name: autoware_motion_utils longitudinal_controller: - node_name: /control/trajectory_follower/controller_node_exe @@ -118,7 +118,7 @@ Control: - node_name: /control/trajectory_follower/controller_node_exe logger_name: autoware_universe_utils - node_name: /control/trajectory_follower/controller_node_exe - logger_name: motion_utils + logger_name: autoware_motion_utils vehicle_cmd_gate: - node_name: /control/vehicle_cmd_gate diff --git a/driving_environment_analyzer/package.xml b/driving_environment_analyzer/package.xml index 542ca888..ec53af64 100644 --- a/driving_environment_analyzer/package.xml +++ b/driving_environment_analyzer/package.xml @@ -17,6 +17,7 @@ autoware_adapi_v1_msgs autoware_behavior_path_planner_common autoware_lane_departure_checker + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -29,7 +30,6 @@ libqt5-core libqt5-gui libqt5-widgets - motion_utils qtbase5-dev rclcpp rclcpp_components diff --git a/driving_environment_analyzer/src/utils.cpp b/driving_environment_analyzer/src/utils.cpp index 33645722..6de04534 100644 --- a/driving_environment_analyzer/src/utils.cpp +++ b/driving_environment_analyzer/src/utils.cpp @@ -14,7 +14,7 @@ #include "driving_environment_analyzer/utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include #include @@ -79,7 +79,7 @@ double calcElevationAngle(const lanelet::ConstLanelet & lane, const Pose & pose) return 0.0; } - const size_t idx = motion_utils::findNearestSegmentIndex(points, pose.position); + const size_t idx = autoware_motion_utils::findNearestSegmentIndex(points, pose.position); const auto p1 = autoware_universe_utils::getPoint(points.at(idx)); const auto p2 = autoware_universe_utils::getPoint(points.at(idx + 1)); @@ -96,7 +96,7 @@ double getLaneWidth(const lanelet::ConstLanelet & lane) return points; }; - const auto lon_length = motion_utils::calcArcLength(to_ros_msg(lane.centerline())); + const auto lon_length = autoware_motion_utils::calcArcLength(to_ros_msg(lane.centerline())); return boost::geometry::area(lane.polygon2d().basicPolygon()) / lon_length; } @@ -118,7 +118,7 @@ double getMaxCurvature(const lanelet::ConstLanelets & lanes) double max_value = 0.0; for (const auto & lane : lanes) { - const auto values = motion_utils::calcCurvature(to_ros_msg(lane.centerline())); + const auto values = autoware_motion_utils::calcCurvature(to_ros_msg(lane.centerline())); const auto max_value_itr = std::max_element(values.begin(), values.end()); if (max_value_itr == values.end()) { continue; @@ -146,7 +146,7 @@ std::pair getLaneWidth(const lanelet::ConstLanelets & lanes) double max_value = 0.0; for (const auto & lane : lanes) { - const auto lon_length = motion_utils::calcArcLength(to_ros_msg(lane.centerline())); + const auto lon_length = autoware_motion_utils::calcArcLength(to_ros_msg(lane.centerline())); const auto width = boost::geometry::area(lane.polygon2d().basicPolygon()) / lon_length; if (min_value > width) { diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp index 362094f1..c79f421c 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -15,8 +15,8 @@ #ifndef PLANNING_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_ #define PLANNING_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "planning_debug_tools/msg/trajectory_debug_info.hpp" #include "planning_debug_tools/util.hpp" #include "rclcpp/rclcpp.hpp" @@ -86,8 +86,8 @@ class TrajectoryAnalyzer TrajectoryDebugInfo data; data.stamp = node_->now(); data.size = points.size(); - data.curvature = motion_utils::calcCurvature(points); - const auto arclength_offset = motion_utils::calcSignedArcLength(points, 0, ego_p); + data.curvature = autoware_motion_utils::calcCurvature(points); + const auto arclength_offset = autoware_motion_utils::calcSignedArcLength(points, 0, ego_p); data.arclength = calcPathArcLengthArray(points, -arclength_offset); data.velocity = getVelocityArray(points); data.acceleration = getAccelerationArray(points); diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp index 05264645..e576fbbc 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -15,8 +15,8 @@ #ifndef PLANNING_DEBUG_TOOLS__UTIL_HPP_ #define PLANNING_DEBUG_TOOLS__UTIL_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/path.hpp" diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index 4f8f4cab..6204c205 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -17,10 +17,10 @@ rosidl_default_generators + autoware_motion_utils autoware_planning_msgs autoware_universe_utils geometry_msgs - motion_utils nav_msgs rclcpp rclcpp_components