diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 77adc8e6479b..df8673cd6872 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -19,7 +19,7 @@ common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/autoware_grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp -common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp diff --git a/common/.pages b/common/.pages index 60300879035d..7a87ae2711e1 100644 --- a/common/.pages +++ b/common/.pages @@ -15,8 +15,8 @@ nav: - 'Glog Component': common/glog_component - 'interpolation': common/interpolation - 'Kalman Filter': common/kalman_filter - - 'Motion Utils': common/motion_utils - - 'Vehicle Utils': common/motion_utils/docs/vehicle/vehicle + - 'Motion Utils': common/autoware_motion_utils + - 'Vehicle Utils': common/autoware_motion_utils/docs/vehicle/vehicle - 'Object Recognition Utils': common/object_recognition_utils - 'OSQP Interface': common/osqp_interface/design/osqp_interface-design - 'Perception Utils': common/perception_utils diff --git a/common/README.md b/common/README.md index 6401aeab7b68..05f85de61286 100644 --- a/common/README.md +++ b/common/README.md @@ -13,4 +13,4 @@ The Autoware.Universe Common folder consists of common and testing libraries tha Some of the commonly used libraries are: 1. `autoware_universe_utils` -2. `motion_utils` +2. `autoware_motion_utils` diff --git a/common/motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt similarity index 55% rename from common/motion_utils/CMakeLists.txt rename to common/autoware_motion_utils/CMakeLists.txt index cd81f16685d7..4c36ef2f4e70 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/autoware_motion_utils/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.14) -project(motion_utils) +project(autoware_motion_utils) find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) -ament_auto_add_library(motion_utils SHARED +ament_auto_add_library(autoware_motion_utils SHARED DIRECTORY src ) @@ -15,10 +15,10 @@ if(BUILD_TESTING) file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(test_motion_utils ${test_files}) + ament_add_ros_isolated_gtest(test_autoware_motion_utils ${test_files}) - target_link_libraries(test_motion_utils - motion_utils + target_link_libraries(test_autoware_motion_utils + autoware_motion_utils ) endif() diff --git a/common/motion_utils/README.md b/common/autoware_motion_utils/README.md similarity index 97% rename from common/motion_utils/README.md rename to common/autoware_motion_utils/README.md index fd18540a9d0c..df4808a6f68d 100644 --- a/common/motion_utils/README.md +++ b/common/autoware_motion_utils/README.md @@ -114,4 +114,4 @@ const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_ Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. -`motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. +`autoware_motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/motion_utils/docs/vehicle/vehicle.md b/common/autoware_motion_utils/docs/vehicle/vehicle.md similarity index 100% rename from common/motion_utils/docs/vehicle/vehicle.md rename to common/autoware_motion_utils/docs/vehicle/vehicle.md diff --git a/common/motion_utils/include/motion_utils/constants.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp similarity index 74% rename from common/motion_utils/include/motion_utils/constants.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp index 63a3439a34b8..278ea3fbcae8 100644 --- a/common/motion_utils/include/motion_utils/constants.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__CONSTANTS_HPP_ -#define MOTION_UTILS__CONSTANTS_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ +#define AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ -namespace motion_utils +namespace autoware_motion_utils { constexpr double overlap_threshold = 0.1; -} // namespace motion_utils +} // namespace autoware_motion_utils -#endif // MOTION_UTILS__CONSTANTS_HPP_ +#endif // AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ diff --git a/common/motion_utils/include/motion_utils/distance/distance.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp similarity index 79% rename from common/motion_utils/include/motion_utils/distance/distance.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp index 0a0d00fc9f26..4b4aec2f5b39 100644 --- a/common/motion_utils/include/motion_utils/distance/distance.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__DISTANCE__DISTANCE_HPP_ -#define MOTION_UTILS__DISTANCE__DISTANCE_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ +#define AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ #include #include @@ -22,12 +22,12 @@ #include #include -namespace motion_utils +namespace autoware_motion_utils { std::optional calcDecelDistWithJerkAndAccConstraints( const double current_vel, const double target_vel, const double current_acc, const double acc_min, const double jerk_acc, const double jerk_dec); -} // namespace motion_utils +} // namespace autoware_motion_utils -#endif // MOTION_UTILS__DISTANCE__DISTANCE_HPP_ +#endif // AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ diff --git a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp similarity index 85% rename from common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp index 077c66f9dd6b..fe8cab2b7a07 100644 --- a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ -#define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ #include #include @@ -24,7 +24,7 @@ #include #include -namespace motion_utils +namespace autoware_motion_utils { using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; @@ -49,6 +49,6 @@ class VelocityFactorInterface VelocityFactor velocity_factor_{}; }; -} // namespace motion_utils +} // namespace autoware_motion_utils -#endif // MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ +#endif // AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp similarity index 88% rename from common/motion_utils/include/motion_utils/marker/marker_helper.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp index 0604eee65372..44cab2ed8b52 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ -#define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ +#define AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ #include @@ -21,7 +21,7 @@ #include -namespace motion_utils +namespace autoware_motion_utils { using geometry_msgs::msg::Pose; @@ -48,6 +48,6 @@ visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( const rclcpp::Time & now, const int32_t id); -} // namespace motion_utils +} // namespace autoware_motion_utils -#endif // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ +#endif // AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp similarity index 89% rename from common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp index be5a70ed7ffc..c2ceaddd1687 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ -#define MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ +#define AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ #include @@ -25,7 +25,7 @@ #include #include -namespace motion_utils +namespace autoware_motion_utils { /// @brief type of virtual wall associated with different marker styles and namespace @@ -76,6 +76,6 @@ class VirtualWallMarkerCreator /// @param now current time to be used for displaying the markers visualization_msgs::msg::MarkerArray create_markers(const rclcpp::Time & now = rclcpp::Time()); }; -} // namespace motion_utils +} // namespace autoware_motion_utils -#endif // MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ +#endif // AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp similarity index 98% rename from common/motion_utils/include/motion_utils/resample/resample.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp index 91322360ce5b..be0a9680ee37 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ +#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -21,7 +21,7 @@ #include -namespace motion_utils +namespace autoware_motion_utils { /** * @brief A resampling function for a path(points). Note that in a default setting, position xy are @@ -234,6 +234,6 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, const bool resample_input_trajectory_stop_point = true); -} // namespace motion_utils +} // namespace autoware_motion_utils -#endif // MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ +#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp similarity index 86% rename from common/motion_utils/include/motion_utils/resample/resample_utils.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp index ad312701faad..f80088645243 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ -#define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ #include "autoware/universe_utils/system/backtrace.hpp" +#include +#include #include -#include -#include #include @@ -29,7 +29,7 @@ constexpr double close_s_threshold = 1e-6; static inline rclcpp::Logger get_logger() { - constexpr const char * logger{"motion_utils.resample_utils"}; + constexpr const char * logger{"autoware_motion_utils.resample_utils"}; return rclcpp::get_logger(logger); } @@ -42,7 +42,7 @@ bool validate_size(const T & points) template bool validate_resampling_range(const T & points, const std::vector & resampling_intervals) { - const double points_length = motion_utils::calcArcLength(points); + const double points_length = autoware_motion_utils::calcArcLength(points); return points_length >= resampling_intervals.back(); } @@ -105,10 +105,10 @@ bool validate_arguments(const T & input_points, const double resampling_interval } // check resampling interval - if (resampling_interval < motion_utils::overlap_threshold) { + if (resampling_interval < autoware_motion_utils::overlap_threshold) { RCLCPP_DEBUG( get_logger(), "invalid argument: resampling interval is less than %f", - motion_utils::overlap_threshold); + autoware_motion_utils::overlap_threshold); autoware_universe_utils::print_backtrace(); return false; } @@ -124,4 +124,4 @@ bool validate_arguments(const T & input_points, const double resampling_interval } } // namespace resample_utils -#endif // MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp similarity index 94% rename from common/motion_utils/include/motion_utils/trajectory/conversion.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp index 8d009730a925..c28f76553f71 100644 --- a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ -#define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ #include "autoware_planning_msgs/msg/detail/path__struct.hpp" #include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp" @@ -23,7 +23,7 @@ #include -namespace motion_utils +namespace autoware_motion_utils { using TrajectoryPoints = std::vector; @@ -115,6 +115,6 @@ inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( return output; } -} // namespace motion_utils +} // namespace autoware_motion_utils -#endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp similarity index 93% rename from common/motion_utils/include/motion_utils/trajectory/interpolation.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp index 86a8048b9eec..18dbefa85918 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ -#define MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ #include "autoware/universe_utils/geometry/geometry.hpp" @@ -25,7 +25,7 @@ #include #include -namespace motion_utils +namespace autoware_motion_utils { /** * @brief An interpolation function that finds the closest interpolated point on the trajectory from @@ -91,6 +91,6 @@ geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double tar return autoware_universe_utils::getPose(points.back()); } -} // namespace motion_utils +} // namespace autoware_motion_utils -#endif // MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp similarity index 86% rename from common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp index d6d4fbf55932..43c8a14d0f3a 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include #include #include -namespace motion_utils +namespace autoware_motion_utils { std::optional> getPathIndexRangeWithLaneId( const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); @@ -41,6 +41,6 @@ size_t findNearestSegmentIndexFromLaneId( tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation = true); -} // namespace motion_utils +} // namespace autoware_motion_utils -#endif // MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp similarity index 98% rename from common/motion_utils/include/motion_utils/trajectory/trajectory.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index d9cb853d7db9..3c6539f02721 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#define MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/geometry/pose_deviation.hpp" @@ -35,11 +35,11 @@ #include #include -namespace motion_utils +namespace autoware_motion_utils { static inline rclcpp::Logger get_logger() { - constexpr const char * logger{"motion_utils.trajectory"}; + constexpr const char * logger{"autoware_motion_utils.trajectory"}; return rclcpp::get_logger(logger); } @@ -52,7 +52,7 @@ void validateNonEmpty(const T & points) { if (points.empty()) { autoware_universe_utils::print_backtrace(); - throw std::invalid_argument("[motion_utils] validateNonEmpty(): Points is empty."); + throw std::invalid_argument("[autoware_motion_utils] validateNonEmpty(): Points is empty."); } } @@ -88,7 +88,8 @@ void validateNonSharpAngle( constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { autoware_universe_utils::print_backtrace(); - throw std::invalid_argument("[motion_utils] validateNonSharpAngle(): Too sharp angle."); + throw std::invalid_argument( + "[autoware_motion_utils] validateNonSharpAngle(): Too sharp angle."); } } @@ -405,7 +406,7 @@ double calcLongitudinalOffsetToSegment( { if (seg_idx >= points.size() - 1) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + ": Failed to calculate longitudinal offset because the given segment index is out of the " "points size."); autoware_universe_utils::print_backtrace(); @@ -434,7 +435,7 @@ double calcLongitudinalOffsetToSegment( if (seg_idx >= overlap_removed_points.size() - 1) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + ": Longitudinal offset calculation is not supported for the same points."); autoware_universe_utils::print_backtrace(); if (throw_exception) { @@ -596,7 +597,7 @@ double calcLateralOffset( if (overlap_removed_points.size() == 1) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + ": Lateral offset calculation is not supported for the same points."); autoware_universe_utils::print_backtrace(); if (throw_exception) { @@ -669,7 +670,7 @@ double calcLateralOffset( if (overlap_removed_points.size() == 1) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + ": Lateral offset calculation is not supported for the same points."); autoware_universe_utils::print_backtrace(); if (throw_exception) { @@ -1074,7 +1075,7 @@ std::optional calcLongitudinalOffsetPoint( if (points.size() - 1 < src_idx) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + " error: The given source index is out of the points size. Failed to calculate longitudinal " "offset."); autoware_universe_utils::print_backtrace(); @@ -1205,7 +1206,7 @@ std::optional calcLongitudinalOffsetPose( if (points.size() - 1 < src_idx) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + " error: The given source index is out of the points size. Failed to calculate longitudinal " "offset."); autoware_universe_utils::print_backtrace(); @@ -1787,8 +1788,8 @@ std::optional insertStopPoint( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) { - const auto insert_idx = - motion_utils::insertTargetPoint(stop_seg_idx, stop_point, points_with_twist, overlap_threshold); + const auto insert_idx = autoware_motion_utils::insertTargetPoint( + stop_seg_idx, stop_point, points_with_twist, overlap_threshold); if (!insert_idx) { return std::nullopt; @@ -2230,13 +2231,13 @@ std::optional calcDistanceToForwardStopPoint( } const auto nearest_segment_idx = - motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); + autoware_motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); if (!nearest_segment_idx) { return std::nullopt; } - const auto stop_idx = motion_utils::searchZeroVelocityIndex( + const auto stop_idx = autoware_motion_utils::searchZeroVelocityIndex( points_with_twist, *nearest_segment_idx + 1, points_with_twist.size()); if (!stop_idx) { @@ -2277,7 +2278,7 @@ T cropForwardPoints( } double sum_length = - -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + -autoware_motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { sum_length += autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (forward_length < sum_length) { @@ -2317,7 +2318,7 @@ T cropBackwardPoints( } double sum_length = - -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + -autoware_motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); for (int i = target_seg_idx; 0 < i; --i) { sum_length -= autoware_universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); if (sum_length < -backward_length) { @@ -2416,7 +2417,7 @@ double calcYawDeviation( if (overlap_removed_points.size() <= 1) { const std::string error_message( - "[motion_utils] " + std::string(__func__) + + "[autoware_motion_utils] " + std::string(__func__) + " Given points size is less than 2. Failed to calculate yaw deviation."); autoware_universe_utils::print_backtrace(); if (throw_exception) { @@ -2486,6 +2487,6 @@ extern template bool isTargetPointFront @@ -23,7 +23,7 @@ #include -namespace motion_utils +namespace autoware_motion_utils { using autoware_planning_msgs::msg::Trajectory; @@ -77,6 +77,6 @@ class VehicleArrivalChecker : public VehicleStopChecker void onTrajectory(const Trajectory::ConstSharedPtr msg); }; -} // namespace motion_utils +} // namespace autoware_motion_utils -#endif // MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ +#endif // AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ diff --git a/common/motion_utils/media/ego_nearest_search.svg b/common/autoware_motion_utils/media/ego_nearest_search.svg similarity index 100% rename from common/motion_utils/media/ego_nearest_search.svg rename to common/autoware_motion_utils/media/ego_nearest_search.svg diff --git a/common/motion_utils/media/segment.svg b/common/autoware_motion_utils/media/segment.svg similarity index 100% rename from common/motion_utils/media/segment.svg rename to common/autoware_motion_utils/media/segment.svg diff --git a/common/motion_utils/package.xml b/common/autoware_motion_utils/package.xml similarity index 94% rename from common/motion_utils/package.xml rename to common/autoware_motion_utils/package.xml index c4ec4227f13e..81fec78b0481 100644 --- a/common/motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -1,9 +1,9 @@ - motion_utils + autoware_motion_utils 0.1.0 - The motion_utils package + The autoware_motion_utils package Satoshi Ota Takayuki Murooka diff --git a/common/motion_utils/src/distance/distance.cpp b/common/autoware_motion_utils/src/distance/distance.cpp similarity index 98% rename from common/motion_utils/src/distance/distance.cpp rename to common/autoware_motion_utils/src/distance/distance.cpp index 3ac37bbc7733..244b894bad47 100644 --- a/common/motion_utils/src/distance/distance.cpp +++ b/common/autoware_motion_utils/src/distance/distance.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/distance/distance.hpp" +#include "autoware/motion_utils/distance/distance.hpp" -namespace motion_utils +namespace autoware_motion_utils { namespace { @@ -269,4 +269,4 @@ std::optional calcDecelDistWithJerkAndAccConstraints( return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); } -} // namespace motion_utils +} // namespace autoware_motion_utils diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp similarity index 86% rename from common/motion_utils/src/factor/velocity_factor_interface.cpp rename to common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp index 6878c056b6f7..f9ae98801801 100644 --- a/common/motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include #include #include -namespace motion_utils +namespace autoware_motion_utils { template void VelocityFactorInterface::set( @@ -31,7 +31,7 @@ void VelocityFactorInterface::set( velocity_factor_.behavior = behavior_; velocity_factor_.pose = stop_pose; velocity_factor_.distance = - static_cast(motion_utils::calcSignedArcLength(points, curr_point, stop_point)); + static_cast(autoware_motion_utils::calcSignedArcLength(points, curr_point, stop_point)); velocity_factor_.status = status; velocity_factor_.detail = detail; } @@ -46,4 +46,4 @@ template void VelocityFactorInterface::set &, const Pose &, const Pose &, const VelocityFactorStatus, const std::string &); -} // namespace motion_utils +} // namespace autoware_motion_utils diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp similarity index 97% rename from common/motion_utils/src/marker/marker_helper.cpp rename to common/autoware_motion_utils/src/marker/marker_helper.cpp index 9a0190051733..5cab196e2232 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" @@ -85,7 +85,7 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( } } // namespace -namespace motion_utils +namespace autoware_motion_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, @@ -140,4 +140,4 @@ visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker( { return createDeletedVirtualWallMarkerArray("dead_line_", now, id); } -} // namespace motion_utils +} // namespace autoware_motion_utils diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp similarity index 86% rename from common/motion_utils/src/marker/virtual_wall_marker_creator.cpp rename to common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp index 34aae096ffa9..658aa6620813 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/marker/virtual_wall_marker_creator.hpp" +#include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" -#include "motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" -namespace motion_utils +namespace autoware_motion_utils { void VirtualWallMarkerCreator::cleanup() @@ -55,13 +55,13 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( for (const auto & virtual_wall : virtual_walls_) { switch (virtual_wall.style) { case stop: - create_fn = motion_utils::createStopVirtualWallMarker; + create_fn = autoware_motion_utils::createStopVirtualWallMarker; break; case slowdown: - create_fn = motion_utils::createSlowDownVirtualWallMarker; + create_fn = autoware_motion_utils::createSlowDownVirtualWallMarker; break; case deadline: - create_fn = motion_utils::createDeadLineVirtualWallMarker; + create_fn = autoware_motion_utils::createDeadLineVirtualWallMarker; break; } auto markers = create_fn( @@ -85,4 +85,4 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( cleanup(); return marker_array; } -} // namespace motion_utils +} // namespace autoware_motion_utils diff --git a/common/motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp similarity index 89% rename from common/motion_utils/src/resample/resample.cpp rename to common/autoware_motion_utils/src/resample/resample.cpp index 1378c8921ee0..f0874264aee7 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/resample/resample_utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" -#include "motion_utils/resample/resample_utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -namespace motion_utils +namespace autoware_motion_utils { std::vector resamplePointVector( const std::vector & points, @@ -91,19 +91,19 @@ std::vector resamplePointVector( const std::vector & points, const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { - const double input_length = motion_utils::calcArcLength(points); + const double input_length = autoware_motion_utils::calcArcLength(points); std::vector resampling_arclength; for (double s = 0.0; s < input_length; s += resample_interval) { resampling_arclength.push_back(s); } if (resampling_arclength.empty()) { - std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; return points; } // Insert terminal point - if (input_length - resampling_arclength.back() < motion_utils::overlap_threshold) { + if (input_length - resampling_arclength.back() < autoware_motion_utils::overlap_threshold) { resampling_arclength.back() = input_length; } else { resampling_arclength.push_back(input_length); @@ -118,7 +118,7 @@ std::vector resamplePoseVector( const bool use_lerp_for_z) { // Remove overlap points for resampling - const auto points = motion_utils::removeOverlapPoints(points_raw); + const auto points = autoware_motion_utils::removeOverlapPoints(points_raw); // validate arguments if (!resample_utils::validate_arguments(points, resampled_arclength)) { @@ -145,7 +145,7 @@ std::vector resamplePoseVector( const bool is_driving_forward = autoware_universe_utils::isDrivingForward(points.at(0), points.at(1)); - motion_utils::insertOrientation(resampled_points, is_driving_forward); + autoware_motion_utils::insertOrientation(resampled_points, is_driving_forward); // Initial orientation is depend on the initial value of the resampled_arclength // when backward driving @@ -160,19 +160,19 @@ std::vector resamplePoseVector( const std::vector & points, const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { - const double input_length = motion_utils::calcArcLength(points); + const double input_length = autoware_motion_utils::calcArcLength(points); std::vector resampling_arclength; for (double s = 0.0; s < input_length; s += resample_interval) { resampling_arclength.push_back(s); } if (resampling_arclength.empty()) { - std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; return points; } // Insert terminal point - if (input_length - resampling_arclength.back() < motion_utils::overlap_threshold) { + if (input_length - resampling_arclength.back() < autoware_motion_utils::overlap_threshold) { resampling_arclength.back() = input_length; } else { resampling_arclength.push_back(input_length); @@ -203,9 +203,9 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( std::fabs(distance_to_resampling_point - resampling_arclength.at(j - 1)); const double dist_to_following_point = std::fabs(resampling_arclength.at(j) - distance_to_resampling_point); - if (dist_to_prev_point < motion_utils::overlap_threshold) { + if (dist_to_prev_point < autoware_motion_utils::overlap_threshold) { resampling_arclength.at(j - 1) = distance_to_resampling_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { + } else if (dist_to_following_point < autoware_motion_utils::overlap_threshold) { resampling_arclength.at(j) = distance_to_resampling_point; } else { resampling_arclength.insert( @@ -271,7 +271,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( } if (input_arclength.back() < resampling_arclength.back()) { - std::cerr << "[motion_utils]: resampled path length is longer than input path length" + std::cerr << "[autoware_motion_utils]: resampled path length is longer than input path length" << std::endl; return input_path; } @@ -328,8 +328,9 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( } if (interpolated_pose.size() != resampling_arclength.size()) { - std::cerr << "[motion_utils]: Resampled pose size is different from resampled arclength" - << std::endl; + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; return input_path; } @@ -369,19 +370,19 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( transformed_input_path.at(i) = input_path.points.at(i).point; } // compute path length - const double input_path_len = motion_utils::calcArcLength(transformed_input_path); + const double input_path_len = autoware_motion_utils::calcArcLength(transformed_input_path); std::vector resampling_arclength; for (double s = 0.0; s < input_path_len; s += resample_interval) { resampling_arclength.push_back(s); } if (resampling_arclength.empty()) { - std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; return input_path; } // Insert terminal point - if (input_path_len - resampling_arclength.back() < motion_utils::overlap_threshold) { + if (input_path_len - resampling_arclength.back() < autoware_motion_utils::overlap_threshold) { resampling_arclength.back() = input_path_len; } else { resampling_arclength.push_back(input_path_len); @@ -390,7 +391,7 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( // Insert stop point if (resample_input_path_stop_point) { const auto distance_to_stop_point = - motion_utils::calcDistanceToForwardStopPoint(transformed_input_path, 0); + autoware_motion_utils::calcDistanceToForwardStopPoint(transformed_input_path, 0); if (distance_to_stop_point && !resampling_arclength.empty()) { for (size_t i = 1; i < resampling_arclength.size(); ++i) { if ( @@ -400,9 +401,9 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); const double dist_to_following_point = std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < motion_utils::overlap_threshold) { + if (dist_to_prev_point < autoware_motion_utils::overlap_threshold) { resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { + } else if (dist_to_following_point < autoware_motion_utils::overlap_threshold) { resampling_arclength.at(i) = *distance_to_stop_point; } else { resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); @@ -478,8 +479,9 @@ autoware_planning_msgs::msg::Path resamplePath( const auto interpolated_heading_rate = lerp(heading_rate); if (interpolated_pose.size() != resampled_arclength.size()) { - std::cerr << "[motion_utils]: Resampled pose size is different from resampled arclength" - << std::endl; + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; return input_path; } @@ -510,19 +512,19 @@ autoware_planning_msgs::msg::Path resamplePath( return input_path; } - const double input_path_len = motion_utils::calcArcLength(input_path.points); + const double input_path_len = autoware_motion_utils::calcArcLength(input_path.points); std::vector resampling_arclength; for (double s = 0.0; s < input_path_len; s += resample_interval) { resampling_arclength.push_back(s); } if (resampling_arclength.empty()) { - std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; return input_path; } // Insert terminal point - if (input_path_len - resampling_arclength.back() < motion_utils::overlap_threshold) { + if (input_path_len - resampling_arclength.back() < autoware_motion_utils::overlap_threshold) { resampling_arclength.back() = input_path_len; } else { resampling_arclength.push_back(input_path_len); @@ -531,7 +533,7 @@ autoware_planning_msgs::msg::Path resamplePath( // Insert stop point if (resample_input_path_stop_point) { const auto distance_to_stop_point = - motion_utils::calcDistanceToForwardStopPoint(input_path.points, 0); + autoware_motion_utils::calcDistanceToForwardStopPoint(input_path.points, 0); if (distance_to_stop_point && !resampling_arclength.empty()) { for (size_t i = 1; i < resampling_arclength.size(); ++i) { if ( @@ -541,9 +543,9 @@ autoware_planning_msgs::msg::Path resamplePath( std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); const double dist_to_following_point = std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < motion_utils::overlap_threshold) { + if (dist_to_prev_point < autoware_motion_utils::overlap_threshold) { resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { + } else if (dist_to_following_point < autoware_motion_utils::overlap_threshold) { resampling_arclength.at(i) = *distance_to_stop_point; } else { resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); @@ -641,8 +643,9 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( const auto interpolated_time_from_start = lerp(time_from_start); if (interpolated_pose.size() != resampled_arclength.size()) { - std::cerr << "[motion_utils]: Resampled pose size is different from resampled arclength" - << std::endl; + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; return input_trajectory; } @@ -675,19 +678,20 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( return input_trajectory; } - const double input_trajectory_len = motion_utils::calcArcLength(input_trajectory.points); + const double input_trajectory_len = autoware_motion_utils::calcArcLength(input_trajectory.points); std::vector resampling_arclength; for (double s = 0.0; s < input_trajectory_len; s += resample_interval) { resampling_arclength.push_back(s); } if (resampling_arclength.empty()) { - std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl; + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; return input_trajectory; } // Insert terminal point - if (input_trajectory_len - resampling_arclength.back() < motion_utils::overlap_threshold) { + if ( + input_trajectory_len - resampling_arclength.back() < autoware_motion_utils::overlap_threshold) { resampling_arclength.back() = input_trajectory_len; } else { resampling_arclength.push_back(input_trajectory_len); @@ -696,7 +700,7 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( // Insert stop point if (resample_input_trajectory_stop_point) { const auto distance_to_stop_point = - motion_utils::calcDistanceToForwardStopPoint(input_trajectory.points, 0); + autoware_motion_utils::calcDistanceToForwardStopPoint(input_trajectory.points, 0); if (distance_to_stop_point && !resampling_arclength.empty()) { for (size_t i = 1; i < resampling_arclength.size(); ++i) { if ( @@ -706,9 +710,9 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); const double dist_to_following_point = std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < motion_utils::overlap_threshold) { + if (dist_to_prev_point < autoware_motion_utils::overlap_threshold) { resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < motion_utils::overlap_threshold) { + } else if (dist_to_following_point < autoware_motion_utils::overlap_threshold) { resampling_arclength.at(i) = *distance_to_stop_point; } else { resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); @@ -724,4 +728,4 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( use_zero_order_hold_for_twist); } -} // namespace motion_utils +} // namespace autoware_motion_utils diff --git a/common/motion_utils/src/trajectory/conversion.cpp b/common/autoware_motion_utils/src/trajectory/conversion.cpp similarity index 94% rename from common/motion_utils/src/trajectory/conversion.cpp rename to common/autoware_motion_utils/src/trajectory/conversion.cpp index e3a221c61a2a..63efc7135cae 100644 --- a/common/motion_utils/src/trajectory/conversion.cpp +++ b/common/autoware_motion_utils/src/trajectory/conversion.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include -namespace motion_utils +namespace autoware_motion_utils { /** * @brief Convert std::vector to @@ -51,4 +51,4 @@ std::vector convertToTrajectoryPoi return output; } -} // namespace motion_utils +} // namespace autoware_motion_utils diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp similarity index 93% rename from common/motion_utils/src/trajectory/interpolation.cpp rename to common/autoware_motion_utils/src/trajectory/interpolation.cpp index 05e61b212032..4b00c6344a99 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "interpolation/linear_interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; -namespace motion_utils +namespace autoware_motion_utils { TrajectoryPoint calcInterpolatedPoint( const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, @@ -37,7 +37,7 @@ TrajectoryPoint calcInterpolatedPoint( return trajectory.points.front(); } - const size_t segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const size_t segment_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( trajectory.points, target_pose, dist_threshold, yaw_threshold); // Calculate interpolation ratio @@ -105,7 +105,7 @@ PathPointWithLaneId calcInterpolatedPoint( return path.points.front(); } - const size_t segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const size_t segment_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, target_pose, dist_threshold, yaw_threshold); // Calculate interpolation ratio @@ -145,4 +145,4 @@ PathPointWithLaneId calcInterpolatedPoint( return interpolated_point; } -} // namespace motion_utils +} // namespace autoware_motion_utils diff --git a/common/motion_utils/src/trajectory/path_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp similarity index 95% rename from common/motion_utils/src/trajectory/path_with_lane_id.cpp rename to common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp index 90a7bff4f47e..52870ba2407d 100644 --- a/common/motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include #include -namespace motion_utils +namespace autoware_motion_utils { std::optional> getPathIndexRangeWithLaneId( @@ -136,4 +136,4 @@ tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( return cog_path; } -} // namespace motion_utils +} // namespace autoware_motion_utils diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp similarity index 99% rename from common/motion_utils/src/trajectory/trajectory.cpp rename to common/autoware_motion_utils/src/trajectory/trajectory.cpp index 42c750e3c109..a35e6ff02b84 100644 --- a/common/motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" -namespace motion_utils +namespace autoware_motion_utils { // @@ -599,4 +599,4 @@ template bool isTargetPointFront -namespace motion_utils +namespace autoware_motion_utils { VehicleStopCheckerBase::VehicleStopCheckerBase(rclcpp::Node * node, double buffer_duration) : clock_(node->get_clock()), logger_(node->get_logger()) @@ -118,18 +118,18 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati } const auto & p = odometry_ptr_->pose.pose.position; - const auto idx = motion_utils::searchZeroVelocityIndex(trajectory_ptr_->points); + const auto idx = autoware_motion_utils::searchZeroVelocityIndex(trajectory_ptr_->points); if (!idx) { return false; } - return std::abs(motion_utils::calcSignedArcLength(trajectory_ptr_->points, p, idx.value())) < - th_arrived_distance_m; + return std::abs(autoware_motion_utils::calcSignedArcLength( + trajectory_ptr_->points, p, idx.value())) < th_arrived_distance_m; } void VehicleArrivalChecker::onTrajectory(const Trajectory::ConstSharedPtr msg) { trajectory_ptr_ = msg; } -} // namespace motion_utils +} // namespace autoware_motion_utils diff --git a/common/motion_utils/test/src/distance/test_distance.cpp b/common/autoware_motion_utils/test/src/distance/test_distance.cpp similarity index 96% rename from common/motion_utils/test/src/distance/test_distance.cpp rename to common/autoware_motion_utils/test/src/distance/test_distance.cpp index 5bcb2c26fe1b..f32a92f5c5cb 100644 --- a/common/motion_utils/test/src/distance/test_distance.cpp +++ b/common/autoware_motion_utils/test/src/distance/test_distance.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/distance/distance.hpp" #include "gtest/gtest.h" -#include "motion_utils/distance/distance.hpp" namespace { -using motion_utils::calcDecelDistWithJerkAndAccConstraints; +using autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints; constexpr double epsilon = 1e-3; diff --git a/common/motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp similarity index 89% rename from common/motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp rename to common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp index 7019a5425ac2..5ddb0b1a1e8f 100644 --- a/common/motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" #include "gtest/gtest.h" -#include "motion_utils/marker/virtual_wall_marker_creator.hpp" namespace { constexpr auto wall_ns_suffix = "_virtual_wall"; @@ -38,9 +38,9 @@ bool has_ns_id( TEST(VirtualWallMarkerCreator, oneWall) { - motion_utils::VirtualWall wall; - motion_utils::VirtualWallMarkerCreator creator; - wall.style = motion_utils::VirtualWallType::stop; + autoware_motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWallMarkerCreator creator; + wall.style = autoware_motion_utils::VirtualWallType::stop; wall.pose.position.x = 1.0; wall.pose.position.y = 2.0; creator.add_virtual_wall(wall); @@ -63,16 +63,16 @@ TEST(VirtualWallMarkerCreator, oneWall) TEST(VirtualWallMarkerCreator, manyWalls) { - motion_utils::VirtualWall wall; - motion_utils::VirtualWallMarkerCreator creator; - wall.style = motion_utils::VirtualWallType::stop; + autoware_motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWallMarkerCreator creator; + wall.style = autoware_motion_utils::VirtualWallType::stop; wall.ns = "ns1_"; creator.add_virtual_wall(wall); creator.add_virtual_wall(wall); creator.add_virtual_wall(wall); wall.ns = "ns2_"; creator.add_virtual_wall(wall); - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware_motion_utils::VirtualWallType::slowdown; wall.ns = "ns2_"; creator.add_virtual_wall(wall); creator.add_virtual_wall(wall); @@ -80,7 +80,7 @@ TEST(VirtualWallMarkerCreator, manyWalls) creator.add_virtual_wall(wall); creator.add_virtual_wall(wall); creator.add_virtual_wall(wall); - wall.style = motion_utils::VirtualWallType::deadline; + wall.style = autoware_motion_utils::VirtualWallType::deadline; wall.ns = "ns1_"; creator.add_virtual_wall(wall); wall.ns = "ns2_"; diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/autoware_motion_utils/test/src/resample/test_resample.cpp similarity index 99% rename from common/motion_utils/test/src/resample/test_resample.cpp rename to common/autoware_motion_utils/test/src/resample/test_resample.cpp index 545a71aa0db0..1f83da0726c4 100644 --- a/common/motion_utils/test/src/resample/test_resample.cpp +++ b/common/autoware_motion_utils/test/src/resample/test_resample.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/constants.hpp" +#include "autoware/motion_utils/resample/resample.hpp" #include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/constants.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "motion_utils/constants.hpp" -#include "motion_utils/resample/resample.hpp" #include #include @@ -177,8 +177,8 @@ std::vector generateArclength(const size_t num_points, const double inte TEST(resample_vector_pose, resample_by_same_interval) { + using autoware_motion_utils::resamplePoseVector; using geometry_msgs::msg::Pose; - using motion_utils::resamplePoseVector; std::vector path(10); for (size_t i = 0; i < 10; ++i) { @@ -220,7 +220,7 @@ TEST(resample_vector_pose, resample_by_same_interval) TEST(resample_path_with_lane_id, resample_path_by_vector) { - using motion_utils::resamplePath; + using autoware_motion_utils::resamplePath; // Output is same as input { auto path = generateTestPathWithLaneId(10, 1.0, 3.0, 1.0, 0.01); @@ -540,7 +540,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) TEST(resample_path_with_lane_id, resample_path_by_vector_backward) { - using motion_utils::resamplePath; + using autoware_motion_utils::resamplePath; { tier4_planning_msgs::msg::PathWithLaneId path; @@ -771,7 +771,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) { - using motion_utils::resamplePath; + using autoware_motion_utils::resamplePath; // Lerp x, y { @@ -1009,7 +1009,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) TEST(resample_path_with_lane_id, resample_path_by_same_interval) { - using motion_utils::resamplePath; + using autoware_motion_utils::resamplePath; // Same point resampling { @@ -1194,7 +1194,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) path.points.at(0).point.longitudinal_velocity_mps = 5.0; path.points.back().point.is_final = true; - const double ds = 1.0 - motion_utils::overlap_threshold; + const double ds = 1.0 - autoware_motion_utils::overlap_threshold; const auto resampled_path = resamplePath(path, ds); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { const auto p = resampled_path.points.at(i); @@ -1564,7 +1564,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) TEST(resample_path, resample_path_by_vector) { - using motion_utils::resamplePath; + using autoware_motion_utils::resamplePath; // Output is same as input { auto path = generateTestPath(10, 1.0, 3.0, 1.0, 0.01); @@ -1812,7 +1812,7 @@ TEST(resample_path, resample_path_by_vector) TEST(resample_path, resample_path_by_vector_backward) { - using motion_utils::resamplePath; + using autoware_motion_utils::resamplePath; { autoware_planning_msgs::msg::Path path; @@ -1990,7 +1990,7 @@ TEST(resample_path, resample_path_by_vector_backward) TEST(resample_path, resample_path_by_vector_non_default) { - using motion_utils::resamplePath; + using autoware_motion_utils::resamplePath; // Lerp x, y { @@ -2175,7 +2175,7 @@ TEST(resample_path, resample_path_by_vector_non_default) TEST(resample_path, resample_path_by_same_interval) { - using motion_utils::resamplePath; + using autoware_motion_utils::resamplePath; // Same point resampling { @@ -2321,7 +2321,7 @@ TEST(resample_path, resample_path_by_same_interval) } path.points.at(0).longitudinal_velocity_mps = 5.0; - const double ds = 1.0 - motion_utils::overlap_threshold; + const double ds = 1.0 - autoware_motion_utils::overlap_threshold; const auto resampled_path = resamplePath(path, ds); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { const auto p = resampled_path.points.at(i); @@ -2633,7 +2633,7 @@ TEST(resample_path, resample_path_by_same_interval) TEST(resample_trajectory, resample_trajectory_by_vector) { - using motion_utils::resampleTrajectory; + using autoware_motion_utils::resampleTrajectory; // Output is same as input { auto traj = generateTestTrajectory(10, 1.0, 3.0, 1.0, 0.01, 0.5); @@ -2891,7 +2891,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) TEST(resample_trajectory, resample_trajectory_by_vector_non_default) { - using motion_utils::resampleTrajectory; + using autoware_motion_utils::resampleTrajectory; // Lerp x, y { @@ -3090,7 +3090,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) TEST(resample_trajectory, resample_trajectory_by_same_interval) { - using motion_utils::resampleTrajectory; + using autoware_motion_utils::resampleTrajectory; // Same point resampling { @@ -3246,7 +3246,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) } traj.points.at(0).longitudinal_velocity_mps = 5.0; - const double ds = 1.0 - motion_utils::overlap_threshold; + const double ds = 1.0 - autoware_motion_utils::overlap_threshold; const auto resampled_traj = resampleTrajectory(traj, ds); for (size_t i = 0; i < resampled_traj.points.size() - 1; ++i) { const auto p = resampled_traj.points.at(i); diff --git a/common/motion_utils/test/src/test_motion_utils.cpp b/common/autoware_motion_utils/test/src/test_motion_utils.cpp similarity index 100% rename from common/motion_utils/test/src/test_motion_utils.cpp rename to common/autoware_motion_utils/test/src/test_motion_utils.cpp diff --git a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp similarity index 95% rename from common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp rename to common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index 4ba7ab9d5d85..6cbc0b1cc3c7 100644 --- a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include #include @@ -66,7 +66,7 @@ TEST(trajectory_benchmark, DISABLED_calcLateralOffset) std::default_random_engine e1(r()); std::uniform_real_distribution uniform_dist(0.0, 1000.0); - using motion_utils::calcLateralOffset; + using autoware_motion_utils::calcLateralOffset; const auto traj = generateTestTrajectory(1000, 1.0, 0.0, 0.0, 0.1); constexpr auto nb_iteration = 10000; diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp similarity index 98% rename from common/motion_utils/test/src/trajectory/test_interpolation.cpp rename to common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp index 7b1a57b22981..8f6fb82f8066 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -124,7 +124,7 @@ T generateTestPath( TEST(Interpolation, interpolate_path_for_trajectory) { - using motion_utils::calcInterpolatedPoint; + using autoware_motion_utils::calcInterpolatedPoint; { autoware_planning_msgs::msg::Trajectory traj; @@ -348,7 +348,7 @@ TEST(Interpolation, interpolate_path_for_trajectory) TEST(Interpolation, interpolate_path_for_path) { - using motion_utils::calcInterpolatedPoint; + using autoware_motion_utils::calcInterpolatedPoint; { tier4_planning_msgs::msg::PathWithLaneId path; @@ -540,7 +540,7 @@ TEST(Interpolation, interpolate_path_for_path) TEST(Interpolation, interpolate_points_with_length) { - using motion_utils::calcInterpolatedPose; + using autoware_motion_utils::calcInterpolatedPose; { autoware_planning_msgs::msg::Trajectory traj; diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp similarity index 94% rename from common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp rename to common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index be35ae4c8713..b9e3a59e39b2 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include @@ -54,7 +54,7 @@ PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { - using motion_utils::getPathIndexRangeWithLaneId; + using autoware_motion_utils::getPathIndexRangeWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; // Usual cases @@ -99,8 +99,8 @@ TEST(path_with_lane_id, getPathIndexRangeWithLaneId) TEST(path_with_lane_id, findNearestIndexFromLaneId) { - using motion_utils::findNearestIndexFromLaneId; - using motion_utils::findNearestSegmentIndexFromLaneId; + using autoware_motion_utils::findNearestIndexFromLaneId; + using autoware_motion_utils::findNearestSegmentIndexFromLaneId; const auto path = generateTestPathWithLaneId(10, 1.0); @@ -164,7 +164,7 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId) // NOTE: This test is temporary for the current implementation. TEST(path_with_lane_id, convertToRearWheelCenter) { - using motion_utils::convertToRearWheelCenter; + using autoware_motion_utils::convertToRearWheelCenter; PathWithLaneId path; diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp similarity index 97% rename from common/motion_utils/test/src/trajectory/test_trajectory.cpp rename to common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp index 9c7281048f9f..f22a2e62efa7 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -94,7 +94,7 @@ void updateTrajectoryVelocityAt(T & points, const size_t idx, const double vel) TEST(trajectory, validateNonEmpty) { - using motion_utils::validateNonEmpty; + using autoware_motion_utils::validateNonEmpty; // Empty EXPECT_THROW(validateNonEmpty(Trajectory{}.points), std::invalid_argument); @@ -106,8 +106,8 @@ TEST(trajectory, validateNonEmpty) TEST(trajectory, validateNonSharpAngle_DefaultThreshold) { + using autoware_motion_utils::validateNonSharpAngle; using autoware_planning_msgs::msg::TrajectoryPoint; - using motion_utils::validateNonSharpAngle; TrajectoryPoint p1; p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); @@ -135,9 +135,9 @@ TEST(trajectory, validateNonSharpAngle_DefaultThreshold) TEST(trajectory, validateNonSharpAngle_SetThreshold) { + using autoware_motion_utils::validateNonSharpAngle; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_universe_utils::pi; - using motion_utils::validateNonSharpAngle; TrajectoryPoint p1; p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); @@ -165,7 +165,7 @@ TEST(trajectory, validateNonSharpAngle_SetThreshold) TEST(trajectory, searchZeroVelocityIndex) { - using motion_utils::searchZeroVelocityIndex; + using autoware_motion_utils::searchZeroVelocityIndex; // Empty EXPECT_FALSE(searchZeroVelocityIndex(Trajectory{}.points)); @@ -244,7 +244,7 @@ TEST(trajectory, searchZeroVelocityIndex) TEST(trajectory, searchZeroVelocityIndex_from_pose) { - using motion_utils::searchZeroVelocityIndex; + using autoware_motion_utils::searchZeroVelocityIndex; // No zero velocity point { @@ -307,7 +307,7 @@ TEST(trajectory, searchZeroVelocityIndex_from_pose) TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) { - using motion_utils::findNearestIndex; + using autoware_motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -338,7 +338,7 @@ TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) { - using motion_utils::findNearestIndex; + using autoware_motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -348,7 +348,7 @@ TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) TEST(trajectory, findNearestIndex_Pose_NoThreshold) { - using motion_utils::findNearestIndex; + using autoware_motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -374,7 +374,7 @@ TEST(trajectory, findNearestIndex_Pose_NoThreshold) TEST(trajectory, findNearestIndex_Pose_DistThreshold) { - using motion_utils::findNearestIndex; + using autoware_motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -390,7 +390,7 @@ TEST(trajectory, findNearestIndex_Pose_DistThreshold) TEST(trajectory, findNearestIndex_Pose_YawThreshold) { - using motion_utils::findNearestIndex; + using autoware_motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); const auto max_d = std::numeric_limits::max(); @@ -409,7 +409,7 @@ TEST(trajectory, findNearestIndex_Pose_YawThreshold) TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) { - using motion_utils::findNearestIndex; + using autoware_motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -423,7 +423,7 @@ TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) TEST(trajectory, findNearestSegmentIndex) { - using motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::findNearestSegmentIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -463,7 +463,7 @@ TEST(trajectory, findNearestSegmentIndex) TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) { - using motion_utils::calcLongitudinalOffsetToSegment; + using autoware_motion_utils::calcLongitudinalOffsetToSegment; const auto traj = generateTestTrajectory(10, 1.0); const bool throw_exception = true; @@ -513,7 +513,7 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) { - using motion_utils::calcLongitudinalOffsetToSegment; + using autoware_motion_utils::calcLongitudinalOffsetToSegment; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -525,7 +525,7 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) TEST(trajectory, calcLateralOffset) { - using motion_utils::calcLateralOffset; + using autoware_motion_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0); const bool throw_exception = true; @@ -566,7 +566,7 @@ TEST(trajectory, calcLateralOffset) TEST(trajectory, calcLateralOffset_without_segment_idx) { - using motion_utils::calcLateralOffset; + using autoware_motion_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0); const bool throw_exception = true; @@ -627,7 +627,7 @@ TEST(trajectory, calcLateralOffset_without_segment_idx) TEST(trajectory, calcLateralOffset_CurveTrajectory) { - using motion_utils::calcLateralOffset; + using autoware_motion_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -638,7 +638,7 @@ TEST(trajectory, calcLateralOffset_CurveTrajectory) TEST(trajectory, calcSignedArcLengthFromIndexToIndex) { - using motion_utils::calcSignedArcLength; + using autoware_motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -661,7 +661,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToIndex) TEST(trajectory, calcSignedArcLengthFromPointToIndex) { - using motion_utils::calcSignedArcLength; + using autoware_motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -690,7 +690,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex) TEST(trajectory, calcSignedArcLengthFromIndexToPoint) { - using motion_utils::calcSignedArcLength; + using autoware_motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -719,7 +719,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToPoint) TEST(trajectory, calcSignedArcLengthFromPointToPoint) { - using motion_utils::calcSignedArcLength; + using autoware_motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -782,7 +782,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToPoint) TEST(trajectory, calcArcLength) { - using motion_utils::calcArcLength; + using autoware_motion_utils::calcArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -795,7 +795,7 @@ TEST(trajectory, calcArcLength) TEST(trajectory, convertToTrajectory) { - using motion_utils::convertToTrajectory; + using autoware_motion_utils::convertToTrajectory; // Size check { @@ -807,7 +807,7 @@ TEST(trajectory, convertToTrajectory) TEST(trajectory, convertToTrajectoryPointArray) { - using motion_utils::convertToTrajectoryPointArray; + using autoware_motion_utils::convertToTrajectoryPointArray; const auto traj_input = generateTestTrajectory(100, 1.0); const auto traj = convertToTrajectoryPointArray(traj_input); @@ -823,7 +823,7 @@ TEST(trajectory, convertToTrajectoryPointArray) TEST(trajectory, calcDistanceToForwardStopPointFromIndex) { - using motion_utils::calcDistanceToForwardStopPoint; + using autoware_motion_utils::calcDistanceToForwardStopPoint; auto traj_input = generateTestTrajectory(100, 1.0, 3.0); traj_input.points.at(50).longitudinal_velocity_mps = 0.0; @@ -881,7 +881,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromIndex) TEST(trajectory, calcDistanceToForwardStopPointFromPose) { - using motion_utils::calcDistanceToForwardStopPoint; + using autoware_motion_utils::calcDistanceToForwardStopPoint; auto traj_input = generateTestTrajectory(100, 1.0, 3.0); traj_input.points.at(50).longitudinal_velocity_mps = 0.0; @@ -965,7 +965,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) { - using motion_utils::calcDistanceToForwardStopPoint; + using autoware_motion_utils::calcDistanceToForwardStopPoint; auto traj_input = generateTestTrajectory(100, 1.0, 3.0); traj_input.points.at(50).longitudinal_velocity_mps = 0.0; @@ -1009,8 +1009,8 @@ TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) { + using autoware_motion_utils::calcDistanceToForwardStopPoint; using autoware_universe_utils::deg2rad; - using motion_utils::calcDistanceToForwardStopPoint; const auto max_d = std::numeric_limits::max(); auto traj_input = generateTestTrajectory(100, 1.0, 3.0); @@ -1061,10 +1061,10 @@ TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) TEST(trajectory, calcLongitudinalOffsetPointFromIndex) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::calcLongitudinalOffsetPoint; + using autoware_motion_utils::calcSignedArcLength; using autoware_universe_utils::getPoint; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPoint; - using motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1137,11 +1137,11 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) TEST(trajectory, calcLongitudinalOffsetPointFromPoint) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::calcLongitudinalOffsetPoint; + using autoware_motion_utils::calcSignedArcLength; using autoware_universe_utils::createPoint; using autoware_universe_utils::getPoint; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPoint; - using motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1215,10 +1215,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::calcLongitudinalOffsetPose; + using autoware_motion_utils::calcSignedArcLength; using autoware_universe_utils::getPoint; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1299,10 +1299,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::calcLongitudinalOffsetPose; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_universe_utils::deg2rad; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; Trajectory traj{}; @@ -1385,10 +1385,10 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::calcLongitudinalOffsetPose; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_universe_utils::deg2rad; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; Trajectory traj{}; @@ -1477,11 +1477,11 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::calcLongitudinalOffsetPose; + using autoware_motion_utils::calcSignedArcLength; using autoware_universe_utils::createPoint; using autoware_universe_utils::getPoint; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1563,12 +1563,12 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::calcLongitudinalOffsetPose; + using autoware_motion_utils::calcLongitudinalOffsetToSegment; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using motion_utils::calcLongitudinalOffsetToSegment; Trajectory traj{}; @@ -1637,12 +1637,12 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::calcLongitudinalOffsetPose; + using autoware_motion_utils::calcLongitudinalOffsetToSegment; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using motion_utils::calcLongitudinalOffsetToSegment; Trajectory traj{}; @@ -1715,13 +1715,13 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) TEST(trajectory, insertTargetPoint) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertTargetPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1928,14 +1928,14 @@ TEST(trajectory, insertTargetPoint) TEST(trajectory, insertTargetPoint_Reverse) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertTargetPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::createQuaternionFromYaw; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; constexpr double overlap_threshold = 1e-4; auto traj = generateTestTrajectory(10, 1.0); @@ -1986,13 +1986,13 @@ TEST(trajectory, insertTargetPoint_Reverse) TEST(trajectory, insertTargetPoint_OverlapThreshold) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertTargetPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; constexpr double overlap_threshold = 1e-4; const auto traj = generateTestTrajectory(10, 1.0); @@ -2079,13 +2079,13 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) TEST(trajectory, insertTargetPoint_Length) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertTargetPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -2317,13 +2317,13 @@ TEST(trajectory, insertTargetPoint_Length) TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertTargetPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -2503,13 +2503,13 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Idx) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertTargetPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; const auto traj = generateTestTrajectory(10, 1.0); @@ -2733,13 +2733,13 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero_Start_Idx) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertTargetPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; const auto traj = generateTestTrajectory(10, 1.0); @@ -2920,13 +2920,13 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero TEST(trajectory, insertTargetPoint_Length_from_a_pose) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertTargetPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -3272,13 +3272,13 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) TEST(trajectory, insertStopPoint_from_a_source_index) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertStopPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertStopPoint; const auto traj = generateTestTrajectory(10, 1.0, 10.0); @@ -3527,13 +3527,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) TEST(trajectory, insertStopPoint_from_front_point) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertStopPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertStopPoint; const auto traj = generateTestTrajectory(10, 1.0, 10.0); @@ -3724,13 +3724,13 @@ TEST(trajectory, insertStopPoint_from_front_point) TEST(trajectory, insertStopPoint_from_a_pose) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertStopPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertStopPoint; const auto traj = generateTestTrajectory(10, 1.0, 10.0); const auto total_length = calcArcLength(traj.points); @@ -4110,13 +4110,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) TEST(trajectory, insertStopPoint_with_pose_and_segment_index) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertStopPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::deg2rad; using autoware_universe_utils::getPose; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertStopPoint; const auto traj = generateTestTrajectory(10, 1.0, 3.0); const auto total_length = calcArcLength(traj.points); @@ -4354,12 +4354,12 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) TEST(trajectory, insertDecelPoint_from_a_point) { + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::findNearestSegmentIndex; + using autoware_motion_utils::insertDecelPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::getLongitudinalVelocity; - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertDecelPoint; const auto traj = generateTestTrajectory(10, 1.0, 10.0); const auto total_length = calcArcLength(traj.points); @@ -4444,9 +4444,9 @@ TEST(trajectory, insertDecelPoint_from_a_point) TEST(trajectory, findFirstNearestIndexWithSoftConstraints) { + using autoware_motion_utils::findFirstNearestIndexWithSoftConstraints; + using autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using autoware_universe_utils::pi; - using motion_utils::findFirstNearestIndexWithSoftConstraints; - using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; const auto traj = generateTestTrajectory(10, 1.0); @@ -4970,7 +4970,7 @@ TEST(trajectory, findFirstNearestIndexWithSoftConstraints) TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointAndSegmentIndex) { - using motion_utils::calcSignedArcLength; + using autoware_motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -5040,7 +5040,7 @@ TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointAndSegmentInd TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex) { - using motion_utils::calcSignedArcLength; + using autoware_motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -5119,7 +5119,7 @@ TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex) TEST(trajectory, removeOverlapPoints) { - using motion_utils::removeOverlapPoints; + using autoware_motion_utils::removeOverlapPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); const auto removed_traj = removeOverlapPoints(traj.points, 0); @@ -5246,7 +5246,7 @@ TEST(trajectory, removeOverlapPoints) TEST(trajectory, cropForwardPoints) { - using motion_utils::cropForwardPoints; + using autoware_motion_utils::cropForwardPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); @@ -5277,7 +5277,7 @@ TEST(trajectory, cropForwardPoints) TEST(trajectory, cropBackwardPoints) { - using motion_utils::cropBackwardPoints; + using autoware_motion_utils::cropBackwardPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); @@ -5308,7 +5308,7 @@ TEST(trajectory, cropBackwardPoints) TEST(trajectory, cropPoints) { - using motion_utils::cropPoints; + using autoware_motion_utils::cropPoints; const auto traj = generateTestTrajectory(10, 1.0, 1.0); @@ -5339,8 +5339,8 @@ TEST(trajectory, cropPoints) TEST(Trajectory, removeFirstInvalidOrientationPoints) { - using motion_utils::insertOrientation; - using motion_utils::removeFirstInvalidOrientationPoints; + using autoware_motion_utils::insertOrientation; + using autoware_motion_utils::removeFirstInvalidOrientationPoints; const double max_yaw_diff = M_PI_2; @@ -5392,8 +5392,8 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) TEST(trajectory, calcYawDeviation) { + using autoware_motion_utils::calcYawDeviation; using autoware_planning_msgs::msg::TrajectoryPoint; - using motion_utils::calcYawDeviation; constexpr double tolerance = 1e-3; @@ -5419,9 +5419,9 @@ TEST(trajectory, calcYawDeviation) TEST(trajectory, isTargetPointFront) { + using autoware_motion_utils::isTargetPointFront; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_universe_utils::createPoint; - using motion_utils::isTargetPointFront; // Generate test trajectory const auto trajectory = generateTestTrajectory(10, 1.0); diff --git a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp similarity index 99% rename from common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp rename to common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp index 826539458175..d18aa8d28332 100644 --- a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp +++ b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/vehicle/vehicle_state_checker.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/vehicle/vehicle_state_checker.hpp" #include "test_vehicle_state_checker_helper.hpp" #include @@ -32,11 +32,11 @@ constexpr double STOP_DURATION_THRESHOLD_600_MS = 0.6; constexpr double STOP_DURATION_THRESHOLD_800_MS = 0.8; constexpr double STOP_DURATION_THRESHOLD_1000_MS = 1.0; +using autoware_motion_utils::VehicleArrivalChecker; +using autoware_motion_utils::VehicleStopChecker; using autoware_universe_utils::createPoint; using autoware_universe_utils::createQuaternion; using autoware_universe_utils::createTranslation; -using motion_utils::VehicleArrivalChecker; -using motion_utils::VehicleStopChecker; using nav_msgs::msg::Odometry; class CheckerNode : public rclcpp::Node diff --git a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp similarity index 100% rename from common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp rename to common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index c4641dc1e0db..1921cbb1c4a2 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -10,9 +10,9 @@ ament_cmake autoware_cmake + autoware_motion_utils autoware_planning_msgs autoware_universe_utils - motion_utils rclcpp rclcpp_components tier4_debug_msgs diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index 798f5df70c59..b503d926df53 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -14,7 +14,7 @@ #include "path_distance_calculator.hpp" -#include +#include #include #include @@ -48,8 +48,8 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio return; } - const double distance = - motion_utils::calcSignedArcLength(path->points, pose->pose.position, path->points.size() - 1); + const double distance = autoware_motion_utils::calcSignedArcLength( + path->points, pose->pose.position, path->points.size() - 1); tier4_debug_msgs::msg::Float64Stamped msg; msg.stamp = pose->header.stamp; diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index f1089e242192..e42d8c5a05a1 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -11,13 +11,13 @@ ament_cmake autoware_cmake + autoware_motion_utils autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils libqt5-core libqt5-gui libqt5-widgets - motion_utils qtbase5-dev rclcpp rviz_common diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index b6dfdd88ded9..1096def70d48 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -15,11 +15,11 @@ #ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ #define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#include #include #include #include #include -#include #include #include @@ -196,7 +196,8 @@ class CollisionDataKeeper const double p_yaw = std::atan2(p_dy, p_dx); const double p_vel = p_dist / p_dt; - const auto nearest_idx = motion_utils::findNearestIndex(path, nearest_collision_point); + const auto nearest_idx = + autoware_motion_utils::findNearestIndex(path, nearest_collision_point); const auto & nearest_path_pose = path.at(nearest_idx); // When the ego moves backwards, the direction of movement axis is reversed const auto & traj_yaw = (current_ego_speed > 0.0) diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index e1bfd66376a0..c822adb51080 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -15,6 +15,7 @@ autoware_cmake autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_system_msgs autoware_universe_utils @@ -22,7 +23,6 @@ autoware_vehicle_msgs diagnostic_updater geometry_msgs - motion_utils nav_msgs pcl_conversions pcl_ros diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 16a451702f5e..29ba9474b6e8 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -520,7 +520,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) } // If path is shorter than minimum path length - while (motion_utils::calcArcLength(path) < min_generated_path_length_) { + while (autoware_motion_utils::calcArcLength(path) < min_generated_path_length_) { curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; @@ -630,7 +630,7 @@ void AEB::createObjectDataUsingPointCloudClusters( for (const auto & p : *points_belonging_to_cluster_hulls) { const auto obj_position = autoware_universe_utils::createPoint(p.x, p.y, p.z); const double obj_arc_length = - motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); + autoware_motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); if (std::isnan(obj_arc_length)) continue; // If the object is behind the ego, we need to use the backward long offset. The distance should diff --git a/control/autoware_control_validator/include/autoware/control_validator/utils.hpp b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp index 2d323e79e655..d64c718df388 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/utils.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ #define AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ -#include +#include #include #include @@ -26,11 +26,11 @@ namespace autoware::control_validator { +using autoware_motion_utils::convertToTrajectory; +using autoware_motion_utils::convertToTrajectoryPointArray; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; -using motion_utils::convertToTrajectory; -using motion_utils::convertToTrajectoryPointArray; using TrajectoryPoints = std::vector; void shiftPose(Pose & pose, double longitudinal); diff --git a/control/autoware_control_validator/package.xml b/control/autoware_control_validator/package.xml index 0b461f7322f8..71fefafbaffb 100644 --- a/control/autoware_control_validator/package.xml +++ b/control/autoware_control_validator/package.xml @@ -20,12 +20,12 @@ autoware_cmake rosidl_default_generators + autoware_motion_utils autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils diagnostic_updater geometry_msgs - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/control/autoware_control_validator/src/debug_marker.cpp b/control/autoware_control_validator/src/debug_marker.cpp index 5a0d15b91a01..7fdd447f075d 100644 --- a/control/autoware_control_validator/src/debug_marker.cpp +++ b/control/autoware_control_validator/src/debug_marker.cpp @@ -14,8 +14,8 @@ #include "autoware/control_validator/debug_marker.hpp" +#include #include -#include #include #include @@ -90,7 +90,7 @@ void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs:: { const auto now = node_->get_clock()->now(); const auto stop_wall_marker = - motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); + autoware_motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); autoware_universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } diff --git a/control/autoware_control_validator/src/utils.cpp b/control/autoware_control_validator/src/utils.cpp index 3dbcd3c4f92a..e60c6ea723b4 100644 --- a/control/autoware_control_validator/src/utils.cpp +++ b/control/autoware_control_validator/src/utils.cpp @@ -14,9 +14,9 @@ #include "autoware/control_validator/utils.hpp" +#include +#include #include -#include -#include #include #include @@ -37,8 +37,8 @@ void insertPointInPredictedTrajectory( TrajectoryPoints & modified_trajectory, const Pose & reference_pose, const TrajectoryPoints & predicted_trajectory) { - const auto point_to_interpolate = - motion_utils::calcInterpolatedPoint(convertToTrajectory(predicted_trajectory), reference_pose); + const auto point_to_interpolate = autoware_motion_utils::calcInterpolatedPoint( + convertToTrajectory(predicted_trajectory), reference_pose); modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate); } @@ -59,8 +59,8 @@ bool removeFrontTrajectoryPoint( bool predicted_trajectory_point_removed = false; for (const auto & point : predicted_trajectory_points) { if ( - motion_utils::calcLongitudinalOffsetToSegment(trajectory_points, 0, point.pose.position) < - 0.0) { + autoware_motion_utils::calcLongitudinalOffsetToSegment( + trajectory_points, 0, point.pose.position) < 0.0) { modified_trajectory_points.erase(modified_trajectory_points.begin()); predicted_trajectory_point_removed = true; @@ -75,7 +75,7 @@ bool removeFrontTrajectoryPoint( Trajectory alignTrajectoryWithReferenceTrajectory( const Trajectory & trajectory, const Trajectory & predicted_trajectory) { - const auto last_seg_length = motion_utils::calcSignedArcLength( + const auto last_seg_length = autoware_motion_utils::calcSignedArcLength( trajectory.points, trajectory.points.size() - 2, trajectory.points.size() - 1); // If no overlapping between trajectory and predicted_trajectory, return empty trajectory @@ -85,9 +85,9 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // predicted_trajectory: p1------------------pN // trajectory: t1------------------tN const bool & is_p_n_before_t1 = - motion_utils::calcLongitudinalOffsetToSegment( + autoware_motion_utils::calcLongitudinalOffsetToSegment( trajectory.points, 0, predicted_trajectory.points.back().pose.position) < 0.0; - const bool & is_p1_behind_t_n = motion_utils::calcLongitudinalOffsetToSegment( + const bool & is_p1_behind_t_n = autoware_motion_utils::calcLongitudinalOffsetToSegment( trajectory.points, trajectory.points.size() - 2, predicted_trajectory.points.front().pose.position) - last_seg_length > @@ -152,9 +152,9 @@ double calcMaxLateralDistance( const auto p0 = autoware_universe_utils::getPoint(point); // find nearest segment const size_t nearest_segment_idx = - motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); - const double temp_dist = std::abs( - motion_utils::calcLateralOffset(reference_trajectory.points, p0, nearest_segment_idx)); + autoware_motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); + const double temp_dist = std::abs(autoware_motion_utils::calcLateralOffset( + reference_trajectory.points, p0, nearest_segment_idx)); if (temp_dist > max_dist) { max_dist = temp_dist; } diff --git a/control/autoware_lane_departure_checker/package.xml b/control/autoware_lane_departure_checker/package.xml index 8901a1b4d542..2a08db728856 100644 --- a/control/autoware_lane_departure_checker/package.xml +++ b/control/autoware_lane_departure_checker/package.xml @@ -14,6 +14,7 @@ autoware_cmake autoware_map_msgs + autoware_motion_utils autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils @@ -22,7 +23,6 @@ eigen geometry_msgs lanelet2_extension - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index e1ef69e97fe0..84de8441c5a4 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -16,11 +16,11 @@ #include "autoware/lane_departure_checker/util/create_vehicle_footprint.hpp" +#include #include #include #include #include -#include #include @@ -30,12 +30,12 @@ #include #include +using autoware_motion_utils::calcArcLength; using autoware_universe_utils::LinearRing2d; using autoware_universe_utils::LineString2d; using autoware_universe_utils::MultiPoint2d; using autoware_universe_utils::MultiPolygon2d; using autoware_universe_utils::Point2d; -using motion_utils::calcArcLength; namespace { @@ -169,7 +169,7 @@ PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold) { - const auto nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto nearest_idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, pose, dist_threshold, yaw_threshold); return autoware_universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); } diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index 0f0778010e24..e25352797a0d 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base autoware_universe_utils @@ -28,7 +29,6 @@ eigen geometry_msgs interpolation - motion_utils osqp_interface rclcpp rclcpp_components diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 8db93efc127e..309704596086 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -14,10 +14,10 @@ #include "autoware/mpc_lateral_controller/mpc.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/mpc_lateral_controller/mpc_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "interpolation/linear_interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -166,10 +166,11 @@ void MPC::setReferenceTrajectory( const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param, const Odometry & current_kinematics) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + const size_t nearest_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + const double ego_offset_to_segment = autoware_motion_utils::calcLongitudinalOffsetToSegment( trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position); const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg); @@ -183,7 +184,7 @@ void MPC::setReferenceTrajectory( } const auto is_forward_shift = - motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints()); + autoware_motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints()); // if driving direction is unknown, use previous value m_is_forward_shift = is_forward_shift ? is_forward_shift.value() : m_is_forward_shift; @@ -389,9 +390,10 @@ MPCTrajectory MPC::applyVelocityDynamicsFilter( return input; } - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); + const size_t nearest_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); MPCTrajectory output = input; MPCUtils::dynamicSmoothingVelocity( @@ -676,7 +678,7 @@ double MPC::getPredictionDeltaTime( { // Calculate the time min_prediction_length ahead from current_pose const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input); - const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t nearest_idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); double sum_dist = 0; diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index e6865db227d4..6aedb74e87c8 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -14,13 +14,13 @@ #include "autoware/mpc_lateral_controller/mpc_lateral_controller.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include "autoware/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" #include "tf2_ros/create_timer_ros.h" @@ -408,7 +408,7 @@ bool MpcLateralController::isStoppedState() const // for the stop state judgement. However, it has been removed since the steering // control was turned off when approaching/exceeding the stop line on a curve or // emergency stop situation and it caused large tracking error. - const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t nearest = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( m_current_trajectory.points, m_current_kinematic_state.pose.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); diff --git a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 9d0102432023..a2dd346a442b 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -14,11 +14,11 @@ #include "autoware/mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -346,7 +346,7 @@ bool calcNearestPoseInterp( return false; } - *nearest_index = motion_utils::findFirstNearestIndexWithSoftConstraints( + *nearest_index = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, self_pose, max_dist, max_yaw); const size_t traj_size = traj.size(); diff --git a/control/autoware_operation_mode_transition_manager/package.xml b/control/autoware_operation_mode_transition_manager/package.xml index 9e188136484c..de514e70a0f2 100644 --- a/control/autoware_operation_mode_transition_manager/package.xml +++ b/control/autoware_operation_mode_transition_manager/package.xml @@ -13,13 +13,13 @@ rosidl_default_generators autoware_control_msgs + autoware_motion_utils autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs component_interface_utils geometry_msgs - motion_utils rclcpp rclcpp_components std_srvs diff --git a/control/autoware_operation_mode_transition_manager/src/state.cpp b/control/autoware_operation_mode_transition_manager/src/state.cpp index 5433611e1df1..ea2f9bf4bf03 100644 --- a/control/autoware_operation_mode_transition_manager/src/state.cpp +++ b/control/autoware_operation_mode_transition_manager/src/state.cpp @@ -16,9 +16,9 @@ #include "util.hpp" +#include #include #include -#include #include #include @@ -26,9 +26,9 @@ namespace autoware::operation_mode_transition_manager { +using autoware_motion_utils::findNearestIndex; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::calcYawDeviation; -using motion_utils::findNearestIndex; AutonomousMode::AutonomousMode(rclcpp::Node * node) : logger_(node->get_logger()), clock_(node->get_clock()) @@ -126,7 +126,7 @@ bool AutonomousMode::isModeChangeCompleted() // check for lateral deviation const auto dist_deviation = - motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position); + autoware_motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position); if (std::isnan(dist_deviation)) { RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); return unstable(); @@ -138,7 +138,7 @@ bool AutonomousMode::isModeChangeCompleted() // check for yaw deviation const auto yaw_deviation = - motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose); + autoware_motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose); if (std::isnan(yaw_deviation)) { RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); return unstable(); diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp index beb06c99bd70..f0e231e354bc 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spherical_linear_interpolation.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" #include @@ -87,12 +87,13 @@ std::pair lerpTrajectoryPoint( const T & points, const Pose & pose, const double max_dist, const double max_yaw) { TrajectoryPoint interpolated_point; - const size_t seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(points, pose, max_dist, max_yaw); + const size_t seg_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, pose, max_dist, max_yaw); const double len_to_interpolated = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); - const double len_segment = motion_utils::calcSignedArcLength(points, seg_idx, seg_idx + 1); + autoware_motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); + const double len_segment = + autoware_motion_utils::calcSignedArcLength(points, seg_idx, seg_idx + 1); const double interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0); { diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index fe5678aee93c..91d77e454b7f 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base autoware_universe_utils @@ -30,7 +31,6 @@ eigen geometry_msgs interpolation - motion_utils rclcpp rclcpp_components std_msgs diff --git a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 8ebf483d108b..2a897f92a0ef 100644 --- a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -58,12 +58,12 @@ bool isValidTrajectory(const Trajectory & traj) double calcStopDistance( const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw) { - const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points); + const auto stop_idx_opt = autoware_motion_utils::searchZeroVelocityIndex(traj.points); const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; - const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const size_t seg_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( traj.points, current_pose, max_dist, max_yaw); - const double signed_length_on_traj = motion_utils::calcSignedArcLength( + const double signed_length_on_traj = autoware_motion_utils::calcSignedArcLength( traj.points, current_pose.position, seg_idx, traj.points.at(end_idx).pose.position, std::min(end_idx, traj.points.size() - 2)); diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 6effe8d13dbd..dda6b9632a76 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -14,9 +14,9 @@ #include "autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -509,11 +509,11 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // ========================================================================================== // Remove overlapped points after inserting the interpolated points control_data.interpolated_traj.points = - motion_utils::removeOverlapPoints(control_data.interpolated_traj.points); - control_data.nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + autoware_motion_utils::removeOverlapPoints(control_data.interpolated_traj.points); + control_data.nearest_idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( control_data.interpolated_traj.points, nearest_point.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); - control_data.target_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + control_data.target_idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( control_data.interpolated_traj.points, target_point.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); @@ -594,7 +594,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist; const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist; - // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity + // NOTE: the same velocity threshold as autoware_motion_utils::searchZeroVelocity static constexpr double vel_epsilon = 1e-3; // Let vehicle start after the steering is converged for control accuracy @@ -971,7 +971,7 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop } const auto traj = control_data.interpolated_traj; - const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj.points); + const auto stop_idx = autoware_motion_utils::searchZeroVelocityIndex(traj.points); if (!stop_idx) { return output_motion; } diff --git a/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 10b55feb9b3e..3c547e762bce 100644 --- a/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "gtest/gtest.h" #include "interpolation/spherical_linear_interpolation.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "tf2/LinearMath/Quaternion.h" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp index f2ef30adaa47..16cb6471deb2 100644 --- a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp @@ -37,9 +37,9 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include -#include -#include +#include +#include +#include #include "autoware_control_msgs/msg/lateral.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/control/autoware_pure_pursuit/package.xml b/control/autoware_pure_pursuit/package.xml index 64468eb7ce1b..fc5c0e41a15a 100644 --- a/control/autoware_pure_pursuit/package.xml +++ b/control/autoware_pure_pursuit/package.xml @@ -15,6 +15,7 @@ autoware_cmake autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base autoware_universe_utils @@ -22,7 +23,6 @@ boost geometry_msgs libboost-dev - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp index 71fc315c92df..8a82e60354f3 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp @@ -158,17 +158,17 @@ void PurePursuitLateralController::setResampledTrajectory() { // Interpolate with constant interval distance. std::vector out_arclength; - const auto input_tp_array = motion_utils::convertToTrajectoryPointArray(trajectory_); - const auto traj_length = motion_utils::calcArcLength(input_tp_array); + const auto input_tp_array = autoware_motion_utils::convertToTrajectoryPointArray(trajectory_); + const auto traj_length = autoware_motion_utils::calcArcLength(input_tp_array); for (double s = 0; s < traj_length; s += param_.resampling_ds) { out_arclength.push_back(s); } - trajectory_resampled_ = - std::make_shared(motion_utils::resampleTrajectory( - motion_utils::convertToTrajectory(input_tp_array), out_arclength)); + trajectory_resampled_ = std::make_shared( + autoware_motion_utils::resampleTrajectory( + autoware_motion_utils::convertToTrajectory(input_tp_array), out_arclength)); trajectory_resampled_->points.back() = trajectory_.points.back(); trajectory_resampled_->header = trajectory_.header; - output_tp_array_ = motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); + output_tp_array_ = autoware_motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); } double PurePursuitLateralController::calcCurvature(const size_t closest_idx) @@ -268,8 +268,8 @@ void PurePursuitLateralController::averageFilterTrajectory( boost::optional PurePursuitLateralController::generatePredictedTrajectory() { - const auto closest_idx_result = - motion_utils::findNearestIndex(output_tp_array_, current_odometry_.pose.pose, 3.0, M_PI_4); + const auto closest_idx_result = autoware_motion_utils::findNearestIndex( + output_tp_array_, current_odometry_.pose.pose, 3.0, M_PI_4); if (!closest_idx_result) { return boost::none; @@ -427,7 +427,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( // Calculate target point for velocity/acceleration const auto closest_idx_result = - motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4); + autoware_motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4); if (!closest_idx_result) { RCLCPP_ERROR(logger_, "cannot find closest waypoint"); return {}; @@ -439,7 +439,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( // calculate the lateral error const double lateral_error = - motion_utils::calcLateralOffset(trajectory_resampled_->points, pose.position); + autoware_motion_utils::calcLateralOffset(trajectory_resampled_->points, pose.position); // calculate the current curvature diff --git a/control/autoware_smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml index 12601504dd23..01b9478aeeae 100644 --- a/control/autoware_smart_mpc_trajectory_follower/package.xml +++ b/control/autoware_smart_mpc_trajectory_follower/package.xml @@ -21,11 +21,11 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_system_msgs autoware_universe_utils autoware_vehicle_msgs - motion_utils pybind11_vendor python3-scipy rclcpp diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index 5925fbebb1e7..fa8f5847d6c3 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils @@ -30,7 +31,6 @@ eigen geometry_msgs interpolation - motion_utils osqp_interface rclcpp rclcpp_components diff --git a/control/autoware_trajectory_follower_node/package.xml b/control/autoware_trajectory_follower_node/package.xml index 3aeb3a814421..55ac989ad4a5 100644 --- a/control/autoware_trajectory_follower_node/package.xml +++ b/control/autoware_trajectory_follower_node/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_motion_utils autoware_mpc_lateral_controller autoware_pid_longitudinal_controller autoware_planning_msgs @@ -29,7 +30,6 @@ autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs - motion_utils rclcpp rclcpp_components visualization_msgs diff --git a/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp index 0bd917b9abf7..81459dc9ef72 100644 --- a/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -14,17 +14,17 @@ #include "autoware/trajectory_follower_node/simple_trajectory_follower.hpp" +#include #include -#include #include namespace simple_trajectory_follower { +using autoware_motion_utils::findNearestIndex; using autoware_universe_utils::calcLateralDeviation; using autoware_universe_utils::calcYawDeviation; -using motion_utils::findNearestIndex; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) diff --git a/control/autoware_vehicle_cmd_gate/package.xml b/control/autoware_vehicle_cmd_gate/package.xml index 6c10d9850594..60bbf23f9d5a 100644 --- a/control/autoware_vehicle_cmd_gate/package.xml +++ b/control/autoware_vehicle_cmd_gate/package.xml @@ -17,6 +17,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_motion_utils autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -24,7 +25,6 @@ component_interface_utils diagnostic_updater geometry_msgs - motion_utils rclcpp rclcpp_components std_srvs diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 19e91d863e8f..08de508dbefc 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -20,12 +20,12 @@ #include "moderate_stop_interface.hpp" #include "vehicle_cmd_filter.hpp" +#include #include #include #include #include #include -#include #include #include @@ -81,7 +81,7 @@ using nav_msgs::msg::Odometry; using EngageMsg = autoware_vehicle_msgs::msg::Engage; using EngageSrv = tier4_external_api_msgs::srv::Engage; -using motion_utils::VehicleStopChecker; +using autoware_motion_utils::VehicleStopChecker; struct Commands { Control control; diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp index 6ecf453bf7ab..db0afc134ff7 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp @@ -15,11 +15,11 @@ #ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ #define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "control_performance_analysis/control_performance_analysis_utils.hpp" #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" #include "control_performance_analysis/msg/float_stamped.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 64c775d66e79..878136d83767 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -25,13 +25,13 @@ builtin_interfaces autoware_control_msgs + autoware_motion_utils autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs libboost-dev - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index a4cca2e2a64d..e72adc3ecdad 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -14,8 +14,8 @@ #include "control_performance_analysis/control_performance_analysis_core.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/trajectory/interpolation.hpp" #include #include @@ -86,7 +86,7 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - const auto closest_segment = motion_utils::findNearestSegmentIndex( + const auto closest_segment = autoware_motion_utils::findNearestSegmentIndex( current_trajectory_ptr_->points, *current_vec_pose_ptr_, p_.acceptable_max_distance_to_waypoint_, p_.acceptable_max_yaw_difference_rad_); @@ -391,7 +391,7 @@ std::optional ControlPerformanceAnalysisCore::findCurveRefIdx() std::pair ControlPerformanceAnalysisCore::calculateClosestPose() { const auto interp_point = - motion_utils::calcInterpolatedPoint(*current_trajectory_ptr_, *current_vec_pose_ptr_); + autoware_motion_utils::calcInterpolatedPoint(*current_trajectory_ptr_, *current_vec_pose_ptr_); const double interp_steering_angle = std::atan(p_.wheelbase_ * estimateCurvature()); diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp index bcc5f7dd4542..01223a4a938b 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -15,12 +15,12 @@ #ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ #define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#include +#include #include #include #include #include -#include -#include #include #include #include diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index 48ff15b89389..5474bb93fbf4 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -15,6 +15,8 @@ #ifndef PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ #define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#include +#include #include #include #include @@ -22,8 +24,6 @@ #include #include #include -#include -#include #include #include diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index a6ad488950c0..05c7a5008db2 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -15,10 +15,10 @@ #ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_ #define PREDICTED_PATH_CHECKER__UTILS_HPP_ +#include #include #include #include -#include #include #include diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index 6c888e1e7d18..153d11107a8c 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -12,6 +12,7 @@ ament_cmake autoware_cmake + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_universe_utils @@ -22,7 +23,6 @@ diagnostic_updater eigen geometry_msgs - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp index e5a17ace712a..bf74bf3c8bfc 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp @@ -14,9 +14,9 @@ #include "predicted_path_checker/debug_marker.hpp" +#include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -28,6 +28,8 @@ #include #include +using autoware_motion_utils::createDeletedStopVirtualWallMarker; +using autoware_motion_utils::createStopVirtualWallMarker; using autoware_universe_utils::appendMarkerArray; using autoware_universe_utils::calcOffsetPose; using autoware_universe_utils::createDefaultMarker; @@ -35,8 +37,6 @@ using autoware_universe_utils::createMarkerColor; using autoware_universe_utils::createMarkerOrientation; using autoware_universe_utils::createMarkerScale; using autoware_universe_utils::createPoint; -using motion_utils::createDeletedStopVirtualWallMarker; -using motion_utils::createStopVirtualWallMarker; namespace autoware::motion::control::predicted_path_checker { diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index 505911dbc5b7..524313181f36 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -14,10 +14,10 @@ #include "predicted_path_checker/predicted_path_checker_node.hpp" +#include +#include #include #include -#include -#include #include #include @@ -241,8 +241,9 @@ void PredictedPathCheckerNode::onTimer() // Convert to trajectory array - TrajectoryPoints predicted_trajectory_array = motion_utils::convertToTrajectoryPointArray( - motion_utils::resampleTrajectory(cut_trajectory, node_param_.resample_interval)); + TrajectoryPoints predicted_trajectory_array = + autoware_motion_utils::convertToTrajectoryPointArray( + autoware_motion_utils::resampleTrajectory(cut_trajectory, node_param_.resample_interval)); // Filter the objects @@ -322,7 +323,7 @@ void PredictedPathCheckerNode::onTimer() // trajectory or not const auto reference_trajectory_array = - motion_utils::convertToTrajectoryPointArray(*reference_trajectory_); + autoware_motion_utils::convertToTrajectoryPointArray(*reference_trajectory_); const auto is_discrete_point = isItDiscretePoint(reference_trajectory_array, predicted_trajectory_array.at(stop_idx)); @@ -366,10 +367,11 @@ TrajectoryPoints PredictedPathCheckerNode::trimTrajectoryFromSelfPose( { TrajectoryPoints output{}; - const size_t min_distance_index = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input, self_pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold) + - 1; + const size_t min_distance_index = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, self_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold) + + 1; for (size_t i = min_distance_index; i < input.size(); ++i) { output.push_back(input.at(i)); @@ -385,9 +387,9 @@ bool PredictedPathCheckerNode::isThereStopPointOnReferenceTrajectory( trimTrajectoryFromSelfPose(reference_trajectory_array, current_pose_.get()->pose); const auto nearest_stop_point_on_ref_trajectory = - motion_utils::findNearestIndex(trimmed_reference_trajectory_array, pose); + autoware_motion_utils::findNearestIndex(trimmed_reference_trajectory_array, pose); - const auto stop_point_on_trajectory = motion_utils::searchZeroVelocityIndex( + const auto stop_point_on_trajectory = autoware_motion_utils::searchZeroVelocityIndex( trimmed_reference_trajectory_array, 0, *nearest_stop_point_on_ref_trajectory); return !!stop_point_on_trajectory; @@ -425,7 +427,7 @@ bool PredictedPathCheckerNode::isItDiscretePoint( const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const { const auto nearest_segment = - motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose); + autoware_motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose); const auto nearest_point = utils::calcInterpolatedPoint( reference_trajectory, collision_point.pose.position, *nearest_segment, false); @@ -480,7 +482,7 @@ Trajectory PredictedPathCheckerNode::cutTrajectory( cut.points.push_back(point); total_length += points_distance; } - motion_utils::removeOverlapPoints(cut.points); + autoware_motion_utils::removeOverlapPoints(cut.points); return cut; } @@ -499,7 +501,7 @@ size_t PredictedPathCheckerNode::insertStopPoint( TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point) { const auto nearest_collision_segment = - motion_utils::findNearestSegmentIndex(trajectory, collision_point); + autoware_motion_utils::findNearestSegmentIndex(trajectory, collision_point); const auto nearest_collision_point = utils::calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true); @@ -553,7 +555,7 @@ void PredictedPathCheckerNode::filterObstacles( // Check is it near to trajectory const double max_length = utils::calcObstacleMaxLength(object.shape); - const size_t seg_idx = motion_utils::findNearestSegmentIndex( + const size_t seg_idx = autoware_motion_utils::findNearestSegmentIndex( traj, object.kinematics.initial_pose_with_covariance.pose.position); const auto p_front = autoware_universe_utils::getPoint(traj.at(seg_idx)); const auto p_back = autoware_universe_utils::getPoint(traj.at(seg_idx + 1)); diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index 1321f1d5af50..0f4cc202f5c0 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -21,10 +21,10 @@ namespace utils { +using autoware_motion_utils::findFirstNearestIndexWithSoftConstraints; +using autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::getRPY; -using motion_utils::findFirstNearestIndexWithSoftConstraints; -using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; // Utils Functions Polygon2d createOneStepPolygon( @@ -160,7 +160,7 @@ std::pair findStopPoint( for (size_t i = collision_idx; i > 0; i--) { distance_point_to_collision = - motion_utils::calcSignedArcLength(trajectory_array, i - 1, collision_idx); + autoware_motion_utils::calcSignedArcLength(trajectory_array, i - 1, collision_idx); if (distance_point_to_collision >= desired_distance_base_link_to_collision) { stop_segment_idx = i - 1; found_stop_point = true; @@ -196,7 +196,7 @@ bool isInBrakeDistance( return false; } - const auto distance_to_obstacle = motion_utils::calcSignedArcLength( + const auto distance_to_obstacle = autoware_motion_utils::calcSignedArcLength( trajectory, trajectory.front().pose.position, trajectory.at(stop_idx).pose.position); const double distance_in_delay = relative_velocity * delay_time_sec + diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index c76b27b00634..49631c566eaf 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -14,10 +14,10 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_planning_msgs autoware_universe_utils diagnostic_msgs - motion_utils pluginlib rclcpp rclcpp_components diff --git a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp index d7c9bbb1308f..a0d8f4076dae 100644 --- a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp @@ -14,9 +14,9 @@ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/geometry/pose_deviation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" namespace control_diagnostics { @@ -26,14 +26,14 @@ using autoware_planning_msgs::msg::Trajectory; double calcLateralDeviation(const Trajectory & traj, const Point & point) { - const size_t nearest_index = motion_utils::findNearestIndex(traj.points, point); + const size_t nearest_index = autoware_motion_utils::findNearestIndex(traj.points, point); return std::abs( autoware_universe_utils::calcLateralDeviation(traj.points[nearest_index].pose, point)); } double calcYawDeviation(const Trajectory & traj, const Pose & pose) { - const size_t nearest_index = motion_utils::findNearestIndex(traj.points, pose.position); + const size_t nearest_index = autoware_motion_utils::findNearestIndex(traj.points, pose.position); return std::abs(autoware_universe_utils::calcYawDeviation(traj.points[nearest_index].pose, pose)); } diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 53502b535395..5bd500f200ea 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -13,13 +13,13 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_universe_utils diagnostic_msgs eigen geometry_msgs - motion_utils nav_msgs pluginlib rclcpp diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index 8935b41aa5bc..b7be356a7c52 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -14,9 +14,9 @@ #include "autoware/planning_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/geometry/pose_deviation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" namespace planning_diagnostics { @@ -37,7 +37,8 @@ Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & tra * need more precise calculation, e.g., lateral distance from spline of the reference traj */ for (TrajectoryPoint p : traj.points) { - const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); + const size_t nearest_index = + autoware_motion_utils::findNearestIndex(ref.points, p.pose.position); stat.add(autoware_universe_utils::calcLateralDeviation( ref.points[nearest_index].pose, p.pose.position)); } @@ -56,7 +57,8 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) * need more precise calculation, e.g., yaw distance from spline of the reference traj */ for (TrajectoryPoint p : traj.points) { - const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); + const size_t nearest_index = + autoware_motion_utils::findNearestIndex(ref.points, p.pose.position); stat.add(autoware_universe_utils::calcYawDeviation(ref.points[nearest_index].pose, p.pose)); } return stat; @@ -72,7 +74,8 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr // TODO(Maxime CLEMENT) need more precise calculation for (TrajectoryPoint p : traj.points) { - const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); + const size_t nearest_index = + autoware_motion_utils::findNearestIndex(ref.points, p.pose.position); stat.add(p.longitudinal_velocity_mps - ref.points[nearest_index].longitudinal_velocity_mps); } return stat; diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index 8be0280bb38f..874f2113b750 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -14,8 +14,8 @@ #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include @@ -66,19 +66,21 @@ Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & tr for (const auto & point : traj2.points) { const auto p0 = autoware_universe_utils::getPoint(point); // find nearest segment - const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(traj1.points, p0); + const size_t nearest_segment_idx = + autoware_motion_utils::findNearestSegmentIndex(traj1.points, p0); double dist; // distance to segment if ( nearest_segment_idx == traj1.points.size() - 2 && - motion_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) > + autoware_motion_utils::calcLongitudinalOffsetToSegment( + traj1.points, nearest_segment_idx, p0) > autoware_universe_utils::calcDistance2d( traj1.points[nearest_segment_idx], traj1.points[nearest_segment_idx + 1])) { // distance to last point dist = autoware_universe_utils::calcDistance2d(traj1.points.back(), p0); } else if ( // NOLINT - nearest_segment_idx == 0 && - motion_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) <= 0) { + nearest_segment_idx == 0 && autoware_motion_utils::calcLongitudinalOffsetToSegment( + traj1.points, nearest_segment_idx, p0) <= 0) { // distance to first point dist = autoware_universe_utils::calcDistance2d(traj1.points.front(), p0); } else { diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 7d2a82f7bc52..d0e35fb24d53 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -14,12 +14,12 @@ #include "autoware/planning_evaluator/metrics_calculator.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/planning_evaluator/metrics/deviation_metrics.hpp" #include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp" #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/trajectory/trajectory.hpp" namespace planning_diagnostics { std::optional> MetricsCalculator::calculate( @@ -122,7 +122,8 @@ Trajectory MetricsCalculator::getLookaheadTrajectory( return traj; } - const auto ego_index = motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); + const auto ego_index = + autoware_motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); Trajectory lookahead_traj; lookahead_traj.header = traj.header; double dist = 0.0; diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index ac8a8d215862..082a9bc2d136 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_perception_msgs autoware_universe_utils autoware_vehicle_info_utils @@ -25,7 +26,6 @@ geometry_msgs glog lanelet2_extension - motion_utils nav_msgs object_recognition_utils pluginlib diff --git a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp index ed3f74658594..ca9266ec6247 100644 --- a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp @@ -17,7 +17,7 @@ #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/geometry/pose_deviation.hpp" -#include +#include namespace perception_diagnostics { @@ -30,7 +30,8 @@ double calcLateralDeviation(const std::vector & ref_path, const Pose & tar return 0.0; } - const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); + const size_t nearest_index = + autoware_motion_utils::findNearestIndex(ref_path, target_pose.position); return std::abs( autoware_universe_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); } @@ -41,7 +42,8 @@ double calcYawDeviation(const std::vector & ref_path, const Pose & target_ return 0.0; } - const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); + const size_t nearest_index = + autoware_motion_utils::findNearestIndex(ref_path, target_pose.position); return std::abs(autoware_universe_utils::calcYawDeviation(ref_path[nearest_index], target_pose)); } @@ -54,7 +56,7 @@ std::vector calcPredictedPathDeviation( return {}; } for (const Pose & p : pred_path.path) { - const size_t nearest_index = motion_utils::findNearestIndex(ref_path, p.position); + const size_t nearest_index = autoware_motion_utils::findNearestIndex(ref_path, p.position); deviations.push_back( autoware_universe_utils::calcDistance2d(ref_path[nearest_index].position, p.position)); } diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 55f459582a25..8732de1fe339 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -14,8 +14,8 @@ #include "perception_online_evaluator/metrics_calculator.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/object_classification.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 8de8a8ac9a1e..5f3ea72f7d40 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -19,12 +19,12 @@ ament_cmake autoware_cmake + autoware_motion_utils autoware_universe_utils component_interface_specs component_interface_utils geometry_msgs map_height_fitter - motion_utils rclcpp rclcpp_components std_srvs diff --git a/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp b/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp index 86c68b4a2dbb..140fb3079de4 100644 --- a/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/stop_check_module.hpp @@ -15,13 +15,13 @@ #ifndef POSE_INITIALIZER__STOP_CHECK_MODULE_HPP_ #define POSE_INITIALIZER__STOP_CHECK_MODULE_HPP_ -#include +#include #include #include #include -class StopCheckModule : public motion_utils::VehicleStopCheckerBase +class StopCheckModule : public autoware_motion_utils::VehicleStopCheckerBase { public: StopCheckModule(rclcpp::Node * node, double buffer_duration); diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml index caf6b83936fa..37bb7aafb01b 100644 --- a/perception/autoware_map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -14,12 +14,12 @@ ament_cmake autoware_cmake + autoware_motion_utils autoware_perception_msgs autoware_universe_utils glog interpolation lanelet2_extension - motion_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index a350873321ce..0916a4568561 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -14,6 +14,8 @@ #include "map_based_prediction/map_based_prediction_node.hpp" +#include +#include #include #include #include @@ -22,8 +24,6 @@ #include #include #include -#include -#include #include @@ -95,7 +95,7 @@ double calcAbsLateralOffset( boundary_path[i] = autoware_universe_utils::createPoint(x, y, 0.0); } - return std::fabs(motion_utils::calcLateralOffset(boundary_path, search_pose.position)); + return std::fabs(autoware_motion_utils::calcLateralOffset(boundary_path, search_pose.position)); } /** @@ -1700,7 +1700,7 @@ float MapBasedPredictionNode::calculateLocalLikelihood( converted_centerline.push_back(converted_p); } const double lat_dist = - std::fabs(motion_utils::calcLateralOffset(converted_centerline, obj_point)); + std::fabs(autoware_motion_utils::calcLateralOffset(converted_centerline, obj_point)); // Compute Chi-squared distributed (Equation (8) in the paper) const double sigma_d = sigma_lateral_offset_; // Standard Deviation for lateral position @@ -2192,7 +2192,7 @@ double MapBasedPredictionNode::calcRightLateralOffset( boundary_path[i] = autoware_universe_utils::createPoint(x, y, 0.0); } - return std::fabs(motion_utils::calcLateralOffset(boundary_path, search_pose.position)); + return std::fabs(autoware_motion_utils::calcLateralOffset(boundary_path, search_pose.position)); } double MapBasedPredictionNode::calcLeftLateralOffset( @@ -2370,7 +2370,7 @@ std::vector MapBasedPredictionNode::convertPathType( // Resample Path const auto resampled_converted_path = - motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); + autoware_motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); converted_paths.push_back(resampled_converted_path); } diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index 2f811e89f529..f28aae2ea979 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -14,9 +14,9 @@ #include "map_based_prediction/path_generator.hpp" +#include #include #include -#include #include @@ -187,7 +187,7 @@ PredictedPath PathGenerator::generatePolynomialPath( const double lateral_duration, const double speed_limit) const { // Get current Frenet Point - const double ref_path_len = motion_utils::calcArcLength(ref_path); + const double ref_path_len = autoware_motion_utils::calcArcLength(ref_path); const auto current_point = getFrenetPoint(object, ref_path, speed_limit, duration); // Step1. Set Target Frenet Point @@ -398,9 +398,10 @@ FrenetPoint PathGenerator::getFrenetPoint( FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; - const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(ref_path, obj_point); - const double l = - motion_utils::calcLongitudinalOffsetToSegment(ref_path, nearest_segment_idx, obj_point); + const size_t nearest_segment_idx = + autoware_motion_utils::findNearestSegmentIndex(ref_path, obj_point); + const double l = autoware_motion_utils::calcLongitudinalOffsetToSegment( + ref_path, nearest_segment_idx, obj_point); const float vx = static_cast(object.kinematics.twist_with_covariance.twist.linear.x); const float vy = static_cast(object.kinematics.twist_with_covariance.twist.linear.y); const float obj_yaw = @@ -479,8 +480,8 @@ FrenetPoint PathGenerator::getFrenetPoint( const float acceleration_adjusted_velocity_x = get_acceleration_adjusted_velocity(vx, ax); const float acceleration_adjusted_velocity_y = get_acceleration_adjusted_velocity(vy, ay); - frenet_point.s = motion_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; - frenet_point.d = motion_utils::calcLateralOffset(ref_path, obj_point); + frenet_point.s = autoware_motion_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; + frenet_point.d = autoware_motion_utils::calcLateralOffset(ref_path, obj_point); frenet_point.s_vel = acceleration_adjusted_velocity_x * std::cos(delta_yaw) - acceleration_adjusted_velocity_y * std::sin(delta_yaw); frenet_point.d_vel = acceleration_adjusted_velocity_x * std::sin(delta_yaw) + diff --git a/planning/autoware_freespace_planner/package.xml b/planning/autoware_freespace_planner/package.xml index b098c7337954..ebc8d9df654f 100644 --- a/planning/autoware_freespace_planner/package.xml +++ b/planning/autoware_freespace_planner/package.xml @@ -16,13 +16,13 @@ autoware_cmake autoware_freespace_planning_algorithms + autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index ca012e7d8531..b0cf556c49b5 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -32,8 +32,8 @@ #include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" +#include #include -#include #include #include @@ -142,7 +142,7 @@ Trajectory getPartialTrajectory( double calcDistance2d(const Trajectory & trajectory, const Pose & pose) { - const auto idx = motion_utils::findNearestIndex(trajectory.points, pose.position); + const auto idx = autoware_motion_utils::findNearestIndex(trajectory.points, pose.position); return autoware_universe_utils::calcDistance2d(trajectory.points.at(idx), pose); } @@ -365,8 +365,8 @@ bool FreespacePlannerNode::isPlanRequired() if (node_param_.replan_when_obstacle_found) { algo_->setMap(*occupancy_grid_); - const size_t nearest_index_partial = - motion_utils::findNearestIndex(partial_trajectory_.points, current_pose_.pose.position); + const size_t nearest_index_partial = autoware_motion_utils::findNearestIndex( + partial_trajectory_.points, current_pose_.pose.position); const size_t end_index_partial = partial_trajectory_.points.size() - 1; const auto forward_trajectory = diff --git a/planning/autoware_mission_planner/package.xml b/planning/autoware_mission_planner/package.xml index 6fb7cca836fd..deaf9b8e942a 100644 --- a/planning/autoware_mission_planner/package.xml +++ b/planning/autoware_mission_planner/package.xml @@ -19,13 +19,13 @@ autoware_adapi_v1_msgs autoware_map_msgs + autoware_motion_utils autoware_planning_msgs autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils geometry_msgs lanelet2_extension - motion_utils pluginlib rclcpp rclcpp_components diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp index 275d5cd82578..edce59e4f473 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -16,6 +16,7 @@ #include "utility_functions.hpp" +#include #include #include #include @@ -25,7 +26,6 @@ #include #include #include -#include #include #include @@ -108,8 +108,8 @@ geometry_msgs::msg::Pose get_closest_centerline_pose( const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, point.position); - const auto nearest_idx = - motion_utils::findNearestIndex(convertCenterlineToPoints(closest_lanelet), point.position); + const auto nearest_idx = autoware_motion_utils::findNearestIndex( + convertCenterlineToPoints(closest_lanelet), point.position); const auto nearest_point = closest_lanelet.centerline()[nearest_idx]; // shift nearest point on its local y axis so that vehicle's right and left edges diff --git a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp index 55790702d5ef..0b892d799807 100644 --- a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp @@ -15,7 +15,7 @@ #ifndef MISSION_PLANNER__ARRIVAL_CHECKER_HPP_ #define MISSION_PLANNER__ARRIVAL_CHECKER_HPP_ -#include +#include #include #include @@ -42,7 +42,7 @@ class ArrivalChecker double duration_; std::optional goal_with_uuid_; rclcpp::Subscription::SharedPtr sub_goal_; - motion_utils::VehicleStopChecker vehicle_stop_checker_; + autoware_motion_utils::VehicleStopChecker vehicle_stop_checker_; }; } // namespace autoware::mission_planner diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index d0f78e695c8c..a7f4787f76c4 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include "autoware/universe_utils/ros/uuid_helper.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include @@ -280,21 +280,21 @@ struct EgoNearestParam TrajectoryPoint calcInterpolatedPoint( const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const { - return motion_utils::calcInterpolatedPoint( - motion_utils::convertToTrajectory(traj_points), pose, dist_threshold, yaw_threshold); + return autoware_motion_utils::calcInterpolatedPoint( + autoware_motion_utils::convertToTrajectory(traj_points), pose, dist_threshold, yaw_threshold); } size_t findIndex( const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const { - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( traj_points, pose, dist_threshold, yaw_threshold); } size_t findSegmentIndex( const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const { - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( traj_points, pose, dist_threshold, yaw_threshold); } diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index e342ed3b7495..525a2cfca6ec 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/obstacle_cruise_planner/common_structs.hpp" #include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -172,7 +172,7 @@ class PlannerInterface const geometry_msgs::msg::Pose & ego_pose) const { const auto & p = ego_nearest_param_; - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( traj_points, ego_pose, p.dist_threshold, p.yaw_threshold); } @@ -181,7 +181,7 @@ class PlannerInterface const geometry_msgs::msg::Pose & ego_pose) const { const auto & p = ego_nearest_param_; - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( traj_points, ego_pose, p.dist_threshold, p.yaw_threshold); } diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 564c4d7cbe6a..6055327a906e 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager @@ -26,7 +27,6 @@ geometry_msgs interpolation lanelet2_extension - motion_utils nav_msgs object_recognition_utils osqp_interface diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 3c64f18cc0f0..8acdfea013e1 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -14,13 +14,13 @@ #include "autoware/obstacle_cruise_planner/node.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/obstacle_cruise_planner/polygon_utils.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include @@ -58,9 +58,9 @@ std::optional calcDistanceToFrontVehicle( const std::vector & traj_points, const size_t ego_idx, const geometry_msgs::msg::Point & obstacle_pos) { - const size_t obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle_pos); + const size_t obstacle_idx = autoware_motion_utils::findNearestIndex(traj_points, obstacle_pos); const auto ego_to_obstacle_distance = - motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx); + autoware_motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx); if (ego_to_obstacle_distance < 0.0) return std::nullopt; return ego_to_obstacle_distance; } @@ -82,7 +82,8 @@ PredictedPath resampleHighestConfidencePredictedPath( double calcDiffAngleAgainstTrajectory( const std::vector & traj_points, const geometry_msgs::msg::Pose & target_pose) { - const size_t nearest_idx = motion_utils::findNearestIndex(traj_points, target_pose.position); + const size_t nearest_idx = + autoware_motion_utils::findNearestIndex(traj_points, target_pose.position); const double traj_yaw = tf2::getYaw(traj_points.at(nearest_idx).pose.orientation); const double target_yaw = tf2::getYaw(target_pose.orientation); @@ -94,7 +95,8 @@ double calcDiffAngleAgainstTrajectory( std::pair projectObstacleVelocityToTrajectory( const std::vector & traj_points, const Obstacle & obstacle) { - const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); + const size_t object_idx = + autoware_motion_utils::findNearestIndex(traj_points, obstacle.pose.position); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); const double obstacle_yaw = tf2::getYaw(obstacle.pose.orientation); @@ -143,7 +145,8 @@ std::vector extendTrajectoryPoints( const double step_length) { auto output_points = input_points; - const auto is_driving_forward_opt = motion_utils::isDrivingForwardWithTwist(input_points); + const auto is_driving_forward_opt = + autoware_motion_utils::isDrivingForwardWithTwist(input_points); const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (extend_distance < std::numeric_limits::epsilon()) { @@ -186,9 +189,9 @@ std::vector getTargetObjectType(rclcpp::Node & node, const std::string & pa std::vector resampleTrajectoryPoints( const std::vector & traj_points, const double interval) { - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware_motion_utils::resampleTrajectory(traj, interval); + return autoware_motion_utils::convertToTrajectoryPointArray(resampled_traj); } geometry_msgs::msg::Point toGeomPoint(const autoware_universe_utils::Point2d & point) @@ -501,7 +504,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto & objects = *objects_ptr; const auto & acc = *acc_ptr; - const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg); + const auto traj_points = autoware_motion_utils::convertToTrajectoryPointArray(*msg); // check if subscribed variables are ready if (traj_points.empty()) { return; @@ -510,7 +513,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms stop_watch_.tic(__func__); *debug_data_ptr_ = DebugData(); - const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(traj_points); + const auto is_driving_forward = autoware_motion_utils::isDrivingForwardWithTwist(traj_points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; // 1. Convert predicted objects to obstacles which are @@ -542,7 +545,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms publishVelocityLimit(slow_down_vel_limit, "slow_down"); // 7. Publish trajectory - const auto output_traj = motion_utils::convertToTrajectory(slow_down_traj_points, msg->header); + const auto output_traj = + autoware_motion_utils::convertToTrajectory(slow_down_traj_points, msg->header); trajectory_pub_->publish(output_traj); // 8. Publish debug data @@ -569,7 +573,7 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( const double vehicle_width = vehicle_info.vehicle_width_m; const size_t nearest_idx = - motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); + autoware_motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); const auto nearest_pose = traj_points.at(nearest_idx).pose; const auto current_ego_pose_error = autoware_universe_utils::inverseTransformPose(current_ego_pose, nearest_pose); @@ -679,7 +683,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( // 3. Check if rough lateral distance is smaller than the threshold const double lat_dist_from_obstacle_to_traj = - motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); + autoware_motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); const double min_lat_dist_to_traj_poly = [&]() { const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); @@ -744,10 +748,11 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle( const std::vector & traj_points, const Obstacle & obstacle, const size_t first_collision_idx) const { - const auto obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); + const auto obstacle_idx = + autoware_motion_utils::findNearestIndex(traj_points, obstacle.pose.position); const double obstacle_to_col_points_distance = - motion_utils::calcSignedArcLength(traj_points, obstacle_idx, first_collision_idx); + autoware_motion_utils::calcSignedArcLength(traj_points, obstacle_idx, first_collision_idx); const double obstacle_max_length = calcObstacleMaxLength(obstacle.shape); // If the obstacle is far in front of the collision point, the obstacle is behind the ego. @@ -939,7 +944,7 @@ std::optional ObstacleCruisePlannerNode::createYieldCruiseObstac } const auto collision_points = [&]() -> std::optional> { - const auto obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose); + const auto obstacle_idx = autoware_motion_utils::findNearestIndex(traj_points, obstacle.pose); if (!obstacle_idx) return std::nullopt; const auto collision_traj_point = traj_points.at(obstacle_idx.value()); const auto object_time = now() + traj_points.at(obstacle_idx.value()).time_from_start; @@ -1353,7 +1358,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl for (const auto & collision_poly : front_collision_polygons) { for (const auto & collision_point : collision_poly.outer()) { const auto collision_geom_point = toGeomPoint(collision_point); - const double dist = motion_utils::calcLongitudinalOffsetToSegment( + const double dist = autoware_motion_utils::calcLongitudinalOffsetToSegment( traj_points, front_seg_idx, collision_geom_point); if (dist < front_min_dist) { front_min_dist = dist; @@ -1368,7 +1373,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl for (const auto & collision_poly : back_collision_polygons) { for (const auto & collision_point : collision_poly.outer()) { const auto collision_geom_point = toGeomPoint(collision_point); - const double dist = motion_utils::calcLongitudinalOffsetToSegment( + const double dist = autoware_motion_utils::calcLongitudinalOffsetToSegment( traj_points, back_seg_idx, collision_geom_point); if (back_max_dist < dist) { back_max_dist = dist; @@ -1451,7 +1456,7 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); const double dist_from_ego_to_obstacle = - std::abs(motion_utils::calcSignedArcLength( + std::abs(autoware_motion_utils::calcSignedArcLength( traj_points, ego_pose.position, collision_points.front().point)) - abs_ego_offset; return dist_from_ego_to_obstacle / std::max(1e-6, std::abs(ego_vel)); diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 032f3c0c536e..b409ed9239f7 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -14,16 +14,16 @@ #include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" -#include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -174,7 +174,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( // Publish Debug trajectories const double traj_front_to_vehicle_offset = - motion_utils::calcSignedArcLength(stop_traj_points, 0, closest_idx); + autoware_motion_utils::calcSignedArcLength(stop_traj_points, 0, closest_idx); publishDebugTrajectory( planner_data, traj_front_to_vehicle_offset, time_vec, *s_boundaries, optimized_result); @@ -217,7 +217,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( } } const auto traj_stop_dist = - motion_utils::calcDistanceToForwardStopPoint(stop_traj_points, closest_idx); + autoware_motion_utils::calcDistanceToForwardStopPoint(stop_traj_points, closest_idx); if (traj_stop_dist) { closest_stop_dist = std::min(*traj_stop_dist + traj_front_to_vehicle_offset, closest_stop_dist); } @@ -226,8 +226,8 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( size_t break_id = stop_traj_points.size(); std::vector resampled_opt_position; for (size_t i = closest_idx; i < stop_traj_points.size(); ++i) { - const double query_s = - std::max(motion_utils::calcSignedArcLength(stop_traj_points, 0, i), opt_position.front()); + const double query_s = std::max( + autoware_motion_utils::calcSignedArcLength(stop_traj_points, 0, i), opt_position.front()); if (query_s > opt_position.back()) { break_id = i; break; @@ -253,7 +253,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( output.back().longitudinal_velocity_mps = 0.0; // terminal velocity is zero // Insert Closest Stop Point - motion_utils::insertStopPoint(0, closest_stop_dist, output); + autoware_motion_utils::insertStopPoint(0, closest_stop_dist, output); prev_output_ = output; return output; @@ -309,7 +309,7 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( TrajectoryPoint prev_output_closest_point; if (smoothed_trajectory_ptr_) { - prev_output_closest_point = motion_utils::calcInterpolatedPoint( + prev_output_closest_point = autoware_motion_utils::calcInterpolatedPoint( *smoothed_trajectory_ptr_, ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); } else { @@ -336,7 +336,7 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( const double engage_vel_thr = engage_velocity_ * engage_exit_ratio_; if (vehicle_speed < engage_vel_thr) { if (target_vel >= engage_velocity_) { - const auto stop_dist = motion_utils::calcDistanceToForwardStopPoint( + const auto stop_dist = autoware_motion_utils::calcDistanceToForwardStopPoint( input_traj_points, ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if ((stop_dist && *stop_dist > stop_dist_to_prohibit_engage_) || !stop_dist) { @@ -372,7 +372,7 @@ bool OptimizationBasedPlanner::checkHasReachedGoal( const PlannerData & planner_data, const std::vector & stop_traj_points) { // If goal is close and current velocity is low, we don't optimize trajectory - const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint( + const auto closest_stop_dist = autoware_motion_utils::calcDistanceToForwardStopPoint( stop_traj_points, planner_data.ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.ego_vel < 0.6) { @@ -428,7 +428,7 @@ std::optional OptimizationBasedPlanner::getSBoundaries( const double rss_dist = calcRSSDistance(planner_data.ego_vel, obj_vel); const auto & safe_distance_margin = longitudinal_info_.safe_distance_margin; - const double ego_obj_length = motion_utils::calcSignedArcLength( + const double ego_obj_length = autoware_motion_utils::calcSignedArcLength( stop_traj_points, planner_data.ego_pose.position, obj.collision_points.front().point); const double slow_down_point_length = ego_obj_length - (rss_dist + safe_distance_margin); @@ -442,13 +442,13 @@ std::optional OptimizationBasedPlanner::getSBoundaries( if (min_slow_down_idx) { const auto & current_time = planner_data.current_time; - const auto marker_pose = motion_utils::calcLongitudinalOffsetPose( + const auto marker_pose = autoware_motion_utils::calcLongitudinalOffsetPose( stop_traj_points, planner_data.ego_pose.position, min_slow_down_point_length); if (marker_pose) { MarkerArray wall_msg; - const auto markers = motion_utils::createSlowDownVirtualWallMarker( + const auto markers = autoware_motion_utils::createSlowDownVirtualWallMarker( marker_pose.value(), "obstacle to follow", current_time, 0); autoware_universe_utils::appendMarkerArray(markers, &wall_msg); @@ -583,7 +583,7 @@ bool OptimizationBasedPlanner::checkOnTrajectory( } const double lateral_offset = - std::fabs(motion_utils::calcLateralOffset(stop_traj_points, point.point)); + std::fabs(autoware_motion_utils::calcLateralOffset(stop_traj_points, point.point)); if (lateral_offset < vehicle_info_.max_lateral_offset_m + 0.1) { return true; @@ -597,10 +597,10 @@ std::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurrentP { const size_t ego_segment_idx = ego_nearest_param_.findSegmentIndex(traj_points, ego_pose); - const double traj_length = motion_utils::calcSignedArcLength( + const double traj_length = autoware_motion_utils::calcSignedArcLength( traj_points, ego_pose.position, ego_segment_idx, traj_points.size() - 1); - const auto dist_to_closest_stop_point = motion_utils::calcDistanceToForwardStopPoint( + const auto dist_to_closest_stop_point = autoware_motion_utils::calcDistanceToForwardStopPoint( traj_points, ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (dist_to_closest_stop_point) { return std::min(traj_length, dist_to_closest_stop_point.value()); diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 01cb454aa469..56201229cdee 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -14,12 +14,12 @@ #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include "interpolation/spline_interpolation.hpp" -#include "motion_utils/marker/marker_helper.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" @@ -313,7 +313,7 @@ std::vector PIDBasedPlanner::planCruise( const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( stop_traj_points, dist_to_rss_wall, ego_idx); - auto markers = motion_utils::createSlowDownVirtualWallMarker( + auto markers = autoware_motion_utils::createSlowDownVirtualWallMarker( stop_traj_points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0); // NOTE: use a different color from slow down one to visualize cruise and slow down // separately. @@ -345,7 +345,7 @@ std::vector PIDBasedPlanner::planCruise( // delete marker const auto markers = - motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); + autoware_motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); return stop_traj_points; @@ -480,7 +480,7 @@ std::vector PIDBasedPlanner::doCruiseWithTrajectory( // set target longitudinal motion const auto prev_traj_closest_point = [&]() -> TrajectoryPoint { // if (smoothed_trajectory_ptr_) { - // return motion_utils::calcInterpolatedPoint( + // return autoware_motion_utils::calcInterpolatedPoint( // *smoothed_trajectory_ptr_, planner_data.ego_pose, nearest_dist_deviation_threshold_, // nearest_yaw_deviation_threshold_); // } @@ -492,7 +492,7 @@ std::vector PIDBasedPlanner::doCruiseWithTrajectory( auto cruise_traj_points = getAccelerationLimitedTrajectory( stop_traj_points, planner_data.ego_pose, v0, a0, target_acc, target_jerk_ratio); - const auto zero_vel_idx_opt = motion_utils::searchZeroVelocityIndex(cruise_traj_points); + const auto zero_vel_idx_opt = autoware_motion_utils::searchZeroVelocityIndex(cruise_traj_points); if (!zero_vel_idx_opt) { return cruise_traj_points; } @@ -529,7 +529,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( }; // calculate sv graph - const double s_traj = motion_utils::calcArcLength(traj_points); + const double s_traj = autoware_motion_utils::calcArcLength(traj_points); // const double t_max = 10.0; const double s_max = 50.0; const double max_sampling_num = 100.0; diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 56cf204e64a5..5ab0ba237ec7 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -14,12 +14,12 @@ #include "autoware/obstacle_cruise_planner/planner_interface.hpp" +#include "autoware/motion_utils/distance/distance.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" -#include "motion_utils/distance/distance.hpp" -#include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include @@ -50,7 +50,7 @@ tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( stop_factor.stop_pose = stop_pose; geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point; stop_factor_point.z = stop_pose.position.z; - stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( + stop_factor.dist_to_stop_pose = autoware_motion_utils::calcSignedArcLength( planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position); stop_factor.stop_factor_points.emplace_back(stop_factor_point); @@ -218,9 +218,9 @@ double calcDecelerationVelocityFromDistanceToTarget( std::vector resampleTrajectoryPoints( const std::vector & traj_points, const double interval) { - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware_motion_utils::resampleTrajectory(traj, interval); + return autoware_motion_utils::convertToTrajectoryPointArray(resampled_traj); } autoware_universe_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) @@ -248,7 +248,7 @@ std::vector PlannerInterface::generateStopTrajectory( // delete marker const auto markers = - motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); + autoware_motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); prev_stop_distance_info_ = std::nullopt; @@ -269,14 +269,14 @@ std::vector PlannerInterface::generateStopTrajectory( const auto ego_segment_idx = ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); const double dist_to_collide_on_ref_traj = - motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + + autoware_motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + stop_obstacle.dist_to_collide_on_decimated_traj; const double desired_margin = [&]() { const double margin_from_obstacle = calculateMarginFromObstacleOnCurve(planner_data, stop_obstacle); // Use terminal margin (terminal_safe_distance_margin) for obstacle stop - const auto ref_traj_length = motion_utils::calcSignedArcLength( + const auto ref_traj_length = autoware_motion_utils::calcSignedArcLength( planner_data.traj_points, 0, planner_data.traj_points.size() - 1); if (dist_to_collide_on_ref_traj > ref_traj_length) { return longitudinal_info_.terminal_safe_distance_margin; @@ -284,11 +284,12 @@ std::vector PlannerInterface::generateStopTrajectory( // If behavior stop point is ahead of the closest_obstacle_stop point within a certain // margin we set closest_obstacle_stop_distance to closest_behavior_stop_distance - const auto closest_behavior_stop_idx = - motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1); + const auto closest_behavior_stop_idx = autoware_motion_utils::searchZeroVelocityIndex( + planner_data.traj_points, ego_segment_idx + 1); if (closest_behavior_stop_idx) { - const double closest_behavior_stop_dist_on_ref_traj = motion_utils::calcSignedArcLength( - planner_data.traj_points, 0, *closest_behavior_stop_idx); + const double closest_behavior_stop_dist_on_ref_traj = + autoware_motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, *closest_behavior_stop_idx); const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj - (dist_to_collide_on_ref_traj - margin_from_obstacle); if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { @@ -328,7 +329,7 @@ std::vector PlannerInterface::generateStopTrajectory( } const double acceptable_stop_pos = - motion_utils::calcSignedArcLength( + autoware_motion_utils::calcSignedArcLength( planner_data.traj_points, 0, planner_data.ego_pose.position) + calcMinimumDistanceToStop( planner_data.ego_vel, longitudinal_info_.limit_max_accel, acceptable_stop_acc.value()); @@ -355,7 +356,7 @@ std::vector PlannerInterface::generateStopTrajectory( if (!determined_zero_vel_dist) { // delete marker const auto markers = - motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); + autoware_motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); prev_stop_distance_info_ = std::nullopt; @@ -369,9 +370,9 @@ std::vector PlannerInterface::generateStopTrajectory( // NOTE: We assume that the current trajectory's front point is ahead of the previous // trajectory's front point. const size_t traj_front_point_prev_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prev_stop_distance_info_->first, planner_data.traj_points.front().pose); - const double diff_dist_front_points = motion_utils::calcSignedArcLength( + const double diff_dist_front_points = autoware_motion_utils::calcSignedArcLength( prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, traj_front_point_prev_seg_idx); @@ -386,10 +387,10 @@ std::vector PlannerInterface::generateStopTrajectory( // Insert stop point auto output_traj_points = planner_data.traj_points; const auto zero_vel_idx = - motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); + autoware_motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); if (zero_vel_idx) { // virtual wall marker for stop obstacle - const auto markers = motion_utils::createStopVirtualWallMarker( + const auto markers = autoware_motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, abs_ego_offset, "", planner_data.is_driving_forward); autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); @@ -405,7 +406,7 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish if ego vehicle will over run against the stop point with a limit acceleration const bool will_over_run = determined_zero_vel_dist.value() > - motion_utils::calcSignedArcLength( + autoware_motion_utils::calcSignedArcLength( planner_data.traj_points, 0, planner_data.ego_pose.position) + determined_stop_obstacle->dist_to_collide_on_decimated_traj + determined_desired_margin.value() + 0.1; @@ -447,8 +448,8 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( : std::abs(vehicle_info_.min_longitudinal_offset_m); // calculate short trajectory points towards obstacle - const size_t obj_segment_idx = - motion_utils::findNearestSegmentIndex(planner_data.traj_points, stop_obstacle.collision_point); + const size_t obj_segment_idx = autoware_motion_utils::findNearestSegmentIndex( + planner_data.traj_points, stop_obstacle.collision_point); std::vector short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)}; double sum_short_traj_length{0.0}; for (int i = obj_segment_idx; 0 <= i; --i) { @@ -511,7 +512,7 @@ double PlannerInterface::calculateMarginFromObstacleOnCurve( const double short_margin_from_obstacle = partial_segment_length + - motion_utils::calcSignedArcLength( + autoware_motion_utils::calcSignedArcLength( resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - abs_ego_offset + additional_safe_distance_margin_on_curve_; @@ -531,9 +532,9 @@ double PlannerInterface::calcDistanceToCollisionPoint( ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); const size_t collision_segment_idx = - motion_utils::findNearestSegmentIndex(planner_data.traj_points, collision_point); + autoware_motion_utils::findNearestSegmentIndex(planner_data.traj_points, collision_point); - const auto dist_to_collision_point = motion_utils::calcSignedArcLength( + const auto dist_to_collision_point = autoware_motion_utils::calcSignedArcLength( planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, collision_point, collision_segment_idx); @@ -552,7 +553,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const double dist_to_ego = [&]() { const size_t ego_seg_idx = ego_nearest_param_.findSegmentIndex(slow_down_traj_points, planner_data.ego_pose); - return motion_utils::calcSignedArcLength( + return autoware_motion_utils::calcSignedArcLength( slow_down_traj_points, 0, planner_data.ego_pose.position, ego_seg_idx); }(); const double abs_ego_offset = planner_data.is_driving_forward @@ -561,7 +562,8 @@ std::vector PlannerInterface::generateSlowDownTrajectory( // define function to insert slow down velocity to trajectory const auto insert_point_in_trajectory = [&](const double lon_dist) -> std::optional { - const auto inserted_idx = motion_utils::insertTargetPoint(0, lon_dist, slow_down_traj_points); + const auto inserted_idx = + autoware_motion_utils::insertTargetPoint(0, lon_dist, slow_down_traj_points); if (inserted_idx) { if (inserted_idx.value() + 1 <= slow_down_traj_points.size() - 1) { // zero-order hold for velocity interpolation @@ -646,7 +648,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( return *slow_down_end_idx; }(); - const auto markers = motion_utils::createSlowDownVirtualWallMarker( + const auto markers = autoware_motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); autoware_universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); @@ -654,14 +656,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( // add debug virtual wall if (slow_down_start_idx) { - const auto markers = motion_utils::createSlowDownVirtualWallMarker( + const auto markers = autoware_motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); autoware_universe_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { - const auto markers = motion_utils::createSlowDownVirtualWallMarker( + const auto markers = autoware_motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); autoware_universe_utils::appendMarkerArray( @@ -726,9 +728,9 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( // calculate distance to collision points const double dist_to_front_collision = - motion_utils::calcSignedArcLength(traj_points, 0, obstacle.front_collision_point); + autoware_motion_utils::calcSignedArcLength(traj_points, 0, obstacle.front_collision_point); const double dist_to_back_collision = - motion_utils::calcSignedArcLength(traj_points, 0, obstacle.back_collision_point); + autoware_motion_utils::calcSignedArcLength(traj_points, 0, obstacle.back_collision_point); // calculate offset distance to first collision considering relative velocity const double relative_vel = planner_data.ego_vel - obstacle.velocity; @@ -771,9 +773,9 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( const auto apply_lowpass_filter = [&](const double dist_to_slow_down, const auto prev_point) { if (prev_output && prev_point) { const size_t seg_idx = - motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); + autoware_motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); const double prev_dist_to_slow_down = - motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); + autoware_motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); return signal_processing::lowpassFilter( dist_to_slow_down, prev_dist_to_slow_down, slow_down_param_.lpf_gain_dist_to_slow_down); } diff --git a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp index 4ae0b8ba5f87..1a032c9f5603 100644 --- a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/obstacle_cruise_planner/polygon_utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/trajectory/trajectory.hpp" namespace { @@ -35,8 +35,8 @@ PointWithStamp calcNearestCollisionPoint( std::vector dist_vec; for (const auto & collision_point : collision_points) { - const double dist = - motion_utils::calcLongitudinalOffsetToSegment(segment_points, 0, collision_point.point); + const double dist = autoware_motion_utils::calcLongitudinalOffsetToSegment( + segment_points, 0, collision_point.point); dist_vec.push_back(dist); } @@ -120,8 +120,9 @@ std::optional> getCollisionPoint( } } return std::make_pair( - *max_collision_point, motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) - - *max_collision_length); + *max_collision_point, + autoware_motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) - + *max_collision_length); } // NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index 7f8755fd043d..d7356c0e43fc 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__NODE_HPP_ #define AUTOWARE__PATH_OPTIMIZER__NODE_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/mpt_optimizer.hpp" #include "autoware/path_optimizer/replan_checker.hpp" @@ -22,7 +23,6 @@ #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -52,7 +52,7 @@ class PathOptimizer : public rclcpp::Node public: bool isDrivingForward(const std::vector & path_points) { - const auto is_driving_forward = motion_utils::isDrivingForward(path_points); + const auto is_driving_forward = autoware_motion_utils::isDrivingForward(path_points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; return is_driving_forward_; } diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp index 9737137ad818..e53cf3789069 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ #define AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/type_alias.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.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 diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp index ef22f668b6a0..acc40573ef54 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.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 @@ -61,7 +61,7 @@ std::optional getPointIndexAfter( } double sum_length = - -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + -autoware_motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); std::optional output_idx{std::nullopt}; @@ -144,7 +144,7 @@ size_t findEgoIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -153,7 +153,7 @@ size_t findEgoSegmentIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -179,7 +179,7 @@ std::optional updateFrontPointForFix( // check if the points_for_fix is longer in front than points const double lon_offset_to_prev_front = - motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); + autoware_motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_DEBUG( rclcpp::get_logger("autoware_path_optimizer.trajectory_utils"), diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 39d967df8f07..0830d534c7cb 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -14,13 +14,13 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation - motion_utils nav_msgs osqp_interface rclcpp diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 38d1dc1309cf..7ecb7543d046 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -14,13 +14,13 @@ #include "autoware/path_optimizer/mpt_optimizer.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/utils/geometry_utils.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" #include @@ -564,7 +564,7 @@ std::vector MPTOptimizer::calcReferencePoints( constexpr double tmp_margin = 10.0; size_t ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); - ref_points = motion_utils::cropPoints( + ref_points = autoware_motion_utils::cropPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, backward_traj_length + tmp_margin); @@ -579,7 +579,7 @@ std::vector MPTOptimizer::calcReferencePoints( // 4. crop backward // NOTE: Start point may change. Spline calculation is required. - ref_points = motion_utils::cropPoints( + ref_points = autoware_motion_utils::cropPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, backward_traj_length); ref_points_spline = SplineInterpolationPoints2d(ref_points); @@ -605,7 +605,7 @@ std::vector MPTOptimizer::calcReferencePoints( updateExtraPoints(ref_points); // 9. crop forward - // ref_points = motion_utils::cropForwardPoints( + // ref_points = autoware_motion_utils::cropForwardPoints( // ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length); if (static_cast(mpt_param_.num_points) < ref_points.size()) { ref_points.resize(mpt_param_.num_points); @@ -1587,7 +1587,7 @@ Eigen::VectorXd MPTOptimizer::calcInitialSolutionForManualWarmStart( Eigen::VectorXd u0 = Eigen::VectorXd::Zero(D_un); - const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t nearest_idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( prev_ref_points, ref_points.front().pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); @@ -1713,17 +1713,17 @@ void MPTOptimizer::publishDebugTrajectories( time_keeper_ptr_->tic(__func__); // reference points - const auto ref_traj = motion_utils::convertToTrajectory( + const auto ref_traj = autoware_motion_utils::convertToTrajectory( trajectory_utils::convertToTrajectoryPoints(ref_points), header); debug_ref_traj_pub_->publish(ref_traj); // fixed reference points const auto fixed_traj_points = extractFixedPoints(ref_points); - const auto fixed_traj = motion_utils::convertToTrajectory(fixed_traj_points, header); + const auto fixed_traj = autoware_motion_utils::convertToTrajectory(fixed_traj_points, header); debug_fixed_traj_pub_->publish(fixed_traj); // mpt points - const auto mpt_traj = motion_utils::convertToTrajectory(mpt_traj_points, header); + const auto mpt_traj = autoware_motion_utils::convertToTrajectory(mpt_traj_points, header); debug_mpt_traj_pub_->publish(mpt_traj); time_keeper_ptr_->toc(__func__, " "); diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index c6c6511c9113..46fb7d0d5bb9 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -14,12 +14,12 @@ #include "autoware/path_optimizer/node.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/trajectory/conversion.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 "rclcpp/time.hpp" #include @@ -56,7 +56,7 @@ Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data void setZeroVelocityAfterStopPoint(std::vector & traj_points) { - const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); + const auto opt_zero_vel_idx = autoware_motion_utils::searchZeroVelocityIndex(traj_points); if (opt_zero_vel_idx) { for (size_t i = opt_zero_vel_idx.value(); i < traj_points.size(); ++i) { traj_points.at(i).longitudinal_velocity_mps = 0.0; @@ -245,7 +245,8 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) "Backward path is NOT supported. Just converting path to trajectory"); const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); - const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); + const auto output_traj_msg = + autoware_motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); return; @@ -278,7 +279,7 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = - motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); + autoware_motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); } @@ -399,7 +400,7 @@ void PathOptimizer::applyInputVelocity( const size_t ego_seg_idx = trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); - const auto cropped_points = motion_utils::cropForwardPoints( + const auto cropped_points = autoware_motion_utils::cropForwardPoints( input_traj_points, ego_pose.position, ego_seg_idx, optimized_traj_length + margin_traj_length); @@ -451,14 +452,15 @@ void PathOptimizer::applyInputVelocity( } // insert stop point explicitly - const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); + const auto stop_idx = + autoware_motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); if (stop_idx) { const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.value()).pose; - // NOTE: motion_utils::findNearestSegmentIndex is used instead of + // NOTE: autoware_motion_utils::findNearestSegmentIndex is used instead of // trajectory_utils::findEgoSegmentIndex // for the case where input_traj_points is much longer than output_traj_points, and the // former has a stop point but the latter will not have. - const auto stop_seg_idx = motion_utils::findNearestSegmentIndex( + const auto stop_seg_idx = autoware_motion_utils::findNearestSegmentIndex( output_traj_points, input_stop_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); @@ -469,10 +471,10 @@ void PathOptimizer::applyInputVelocity( } if (*stop_seg_idx == output_traj_points.size() - 2) { const double signed_projected_length_to_segment = - motion_utils::calcLongitudinalOffsetToSegment( + autoware_motion_utils::calcLongitudinalOffsetToSegment( output_traj_points, *stop_seg_idx, input_stop_pose.position); - const double segment_length = - motion_utils::calcSignedArcLength(output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); + const double segment_length = autoware_motion_utils::calcSignedArcLength( + output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); if (segment_length < signed_projected_length_to_segment) { // NOTE: input_stop_pose is outside output_traj_points. return false; @@ -531,10 +533,10 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( debug_data_ptr_->stop_pose_by_drivable_area = optimized_traj_points.at(*first_outside_idx).pose; const auto stop_idx = [&]() { const auto dist = - motion_utils::calcSignedArcLength(optimized_traj_points, 0, *first_outside_idx); + autoware_motion_utils::calcSignedArcLength(optimized_traj_points, 0, *first_outside_idx); const auto dist_with_margin = dist - vehicle_stop_margin_outside_drivable_area_; const auto first_outside_idx_with_margin = - motion_utils::insertTargetPoint(0, dist_with_margin, optimized_traj_points); + autoware_motion_utils::insertTargetPoint(0, dist_with_margin, optimized_traj_points); if (first_outside_idx_with_margin) { return *first_outside_idx_with_margin; } @@ -559,7 +561,7 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos { time_keeper_ptr_->tic(__func__); - auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker( + auto virtual_wall_marker = autoware_motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); if (!enable_outside_drivable_area_stop_) { virtual_wall_marker.markers.front().color = @@ -662,7 +664,7 @@ void PathOptimizer::publishDebugData(const Header & header) const // publish trajectories const auto debug_extended_traj = - motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); + autoware_motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); debug_extended_traj_pub_->publish(debug_extended_traj); time_keeper_ptr_->toc(__func__, " "); diff --git a/planning/autoware_path_optimizer/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp index dda0a9142d3d..9228631b80af 100644 --- a/planning/autoware_path_optimizer/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -14,10 +14,10 @@ #include "autoware/path_optimizer/replan_checker.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include @@ -144,14 +144,14 @@ bool ReplanChecker::isPathAroundEgoChanged( // calculate ego's lateral offset to previous trajectory points const auto prev_ego_seg_idx = trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); - const double prev_ego_lat_offset = - motion_utils::calcLateralOffset(prev_traj_points, p.ego_pose.position, prev_ego_seg_idx); + const double prev_ego_lat_offset = autoware_motion_utils::calcLateralOffset( + prev_traj_points, p.ego_pose.position, prev_ego_seg_idx); // calculate ego's lateral offset to current trajectory points const auto ego_seg_idx = trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); const double ego_lat_offset = - motion_utils::calcLateralOffset(p.traj_points, p.ego_pose.position, ego_seg_idx); + autoware_motion_utils::calcLateralOffset(p.traj_points, p.ego_pose.position, ego_seg_idx); const double diff_ego_lat_offset = prev_ego_lat_offset - ego_lat_offset; if (std::abs(diff_ego_lat_offset) < max_path_shape_around_ego_lat_dist_) { @@ -174,17 +174,17 @@ bool ReplanChecker::isPathForwardChanged( constexpr double lon_dist_interval = 10.0; for (double lon_dist = lon_dist_interval; lon_dist <= max_path_shape_forward_lon_dist_; lon_dist += lon_dist_interval) { - const auto prev_forward_point = - motion_utils::calcLongitudinalOffsetPoint(prev_traj_points, prev_ego_seg_idx, lon_dist); + const auto prev_forward_point = autoware_motion_utils::calcLongitudinalOffsetPoint( + prev_traj_points, prev_ego_seg_idx, lon_dist); if (!prev_forward_point) { continue; } // calculate lateral offset of current trajectory points to prev forward point const auto forward_seg_idx = - motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + autoware_motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); const double forward_lat_offset = - motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); + autoware_motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); if (max_path_shape_forward_lat_dist_ < std::abs(forward_lat_offset)) { return true; } diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 4b51a9cee528..a94f8b4bbabe 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -14,8 +14,8 @@ #include "autoware/path_optimizer/utils/geometry_utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/mpt_optimizer.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" #include @@ -49,7 +49,7 @@ namespace geometry_msgs::msg::Point getStartPoint( const std::vector & bound, const geometry_msgs::msg::Point & point) { - const size_t segment_idx = motion_utils::findNearestSegmentIndex(bound, point); + const size_t segment_idx = autoware_motion_utils::findNearestSegmentIndex(bound, point); const auto & curr_seg_point = bound.at(segment_idx); const auto & next_seg_point = bound.at(segment_idx); const Eigen::Vector2d first_to_target{point.x - curr_seg_point.x, point.y - curr_seg_point.y}; @@ -61,7 +61,8 @@ geometry_msgs::msg::Point getStartPoint( return bound.front(); } - const auto first_point = motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); + const auto first_point = + autoware_motion_utils::calcLongitudinalOffsetPoint(bound, segment_idx, length); if (first_point) { return *first_point; } @@ -84,7 +85,8 @@ bool isFrontDrivableArea( // ignore point behind of the front line const std::vector front_bound = {left_start_point, right_start_point}; - const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); + const double lat_dist_to_front_bound = + autoware_motion_utils::calcLateralOffset(front_bound, point); if (lat_dist_to_front_bound < min_dist) { return true; } diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index fe377b60f503..a1b429ae2357 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -14,11 +14,11 @@ #include "autoware/path_optimizer/utils/trajectory_utils.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.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 "tf2/utils.h" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -140,10 +140,10 @@ std::vector resampleTrajectoryPoints( { constexpr bool enable_resampling_stop_point = true; - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory( + const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware_motion_utils::resampleTrajectory( traj, interval, false, true, true, enable_resampling_stop_point); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware_motion_utils::convertToTrajectoryPointArray(resampled_traj); } // NOTE: stop point will not be resampled @@ -152,10 +152,10 @@ std::vector resampleTrajectoryPointsWithoutStopPoint( { constexpr bool enable_resampling_stop_point = false; - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory( + const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware_motion_utils::resampleTrajectory( traj, interval, false, true, true, enable_resampling_stop_point); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware_motion_utils::convertToTrajectoryPointArray(resampled_traj); } std::vector resampleReferencePoints( @@ -220,7 +220,7 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx) { - const double offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + const double offset_to_segment = autoware_motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); const auto traj_spline = SplineInterpolationPoints2d(traj_points); diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp index defaae9977b7..6a2c3ef45f66 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band_smoother.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ #define AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_smoother/common_structs.hpp" #include "autoware/path_smoother/elastic_band.hpp" #include "autoware/path_smoother/replan_checker.hpp" #include "autoware/path_smoother/type_alias.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -49,7 +49,7 @@ class ElasticBandSmoother : public rclcpp::Node public: bool isDrivingForward(const std::vector & path_points) { - const auto is_driving_forward = motion_utils::isDrivingForward(path_points); + const auto is_driving_forward = autoware_motion_utils::isDrivingForward(path_points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; return is_driving_forward_; } diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp index 6c76593b7832..715a3c35609a 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_smoother/common_structs.hpp" #include "autoware/path_smoother/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 @@ -49,7 +49,7 @@ std::optional getPointIndexAfter( } double sum_length = - -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + -autoware_motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); std::optional output_idx{std::nullopt}; @@ -109,7 +109,7 @@ size_t findEgoSegmentIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -134,7 +134,7 @@ std::optional updateFrontPointForFix( // check if the points_for_fix is longer in front than points const double lon_offset_to_prev_front = - motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); + autoware_motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_DEBUG( rclcpp::get_logger("autoware_path_smoother.trajectory_utils"), diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index 62a59a79c2bc..6cecef433fc3 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -14,12 +14,12 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils geometry_msgs interpolation - motion_utils nav_msgs osqp_interface rclcpp diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index 151a0252945f..b0c5974eca03 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -14,11 +14,11 @@ #include "autoware/path_smoother/elastic_band.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_smoother/type_alias.hpp" #include "autoware/path_smoother/utils/geometry_utils.hpp" #include "autoware/path_smoother/utils/trajectory_utils.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" #include @@ -212,7 +212,7 @@ std::vector EBPathSmoother::smoothTrajectory( const size_t ego_seg_idx = trajectory_utils::findEgoSegmentIndex(traj_points, ego_pose, ego_nearest_param_); - const auto cropped_traj_points = motion_utils::cropPoints( + const auto cropped_traj_points = autoware_motion_utils::cropPoints( traj_points, ego_pose.position, ego_seg_idx, forward_traj_length, backward_traj_length); // check if goal is contained in cropped_traj_points @@ -263,7 +263,7 @@ std::vector EBPathSmoother::smoothTrajectory( // 8. publish eb trajectory const auto eb_traj = - motion_utils::convertToTrajectory(*eb_traj_points, createHeader(clock_.now())); + autoware_motion_utils::convertToTrajectory(*eb_traj_points, createHeader(clock_.now())); debug_eb_traj_pub_->publish(eb_traj); time_keeper_ptr_->toc(__func__, " "); @@ -390,7 +390,7 @@ void EBPathSmoother::updateConstraint( // publish fixed trajectory const auto eb_fixed_traj = - motion_utils::convertToTrajectory(debug_fixed_traj_points, createHeader(clock_.now())); + autoware_motion_utils::convertToTrajectory(debug_fixed_traj_points, createHeader(clock_.now())); debug_eb_fixed_traj_pub_->publish(eb_fixed_traj); time_keeper_ptr_->toc(__func__, " "); @@ -448,7 +448,7 @@ std::optional> EBPathSmoother::convertOptimizedPoin } // update orientation - motion_utils::insertOrientation(eb_traj_points, true); + autoware_motion_utils::insertOrientation(eb_traj_points, true); time_keeper_ptr_->toc(__func__, " "); return eb_traj_points; diff --git a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp index e5f30784a97c..8814fb5f9e5f 100644 --- a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp +++ b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp @@ -14,10 +14,10 @@ #include "autoware/path_smoother/elastic_band_smoother.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/path_smoother/utils/geometry_utils.hpp" #include "autoware/path_smoother/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "rclcpp/time.hpp" #include @@ -54,7 +54,7 @@ Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data void setZeroVelocityAfterStopPoint(std::vector & traj_points) { - const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); + const auto opt_zero_vel_idx = autoware_motion_utils::searchZeroVelocityIndex(traj_points); if (opt_zero_vel_idx) { for (size_t i = opt_zero_vel_idx.value(); i < traj_points.size(); ++i) { traj_points.at(i).longitudinal_velocity_mps = 0.0; @@ -169,7 +169,8 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) "Backward path is NOT supported. Just converting path to trajectory"); const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); - const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); + const auto output_traj_msg = + autoware_motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); path_pub_->publish(*path_ptr); published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp); @@ -223,7 +224,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = - motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); + autoware_motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); const auto output_path_msg = trajectory_utils::create_path(*path_ptr, full_traj_points); path_pub_->publish(output_path_msg); @@ -260,12 +261,12 @@ void ElasticBandSmoother::applyInputVelocity( time_keeper_ptr_->tic(__func__); // crop forward for faster calculation - const double output_traj_length = motion_utils::calcArcLength(output_traj_points); + const double output_traj_length = autoware_motion_utils::calcArcLength(output_traj_points); constexpr double margin_traj_length = 10.0; const auto forward_cropped_input_traj_points = [&]() { const size_t ego_seg_idx = trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); - return motion_utils::cropForwardPoints( + return autoware_motion_utils::cropForwardPoints( input_traj_points, ego_pose.position, ego_seg_idx, output_traj_length + margin_traj_length); }(); @@ -287,14 +288,15 @@ void ElasticBandSmoother::applyInputVelocity( } // insert stop point explicitly - const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); + const auto stop_idx = + autoware_motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); if (stop_idx) { const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.value()).pose; - // NOTE: motion_utils::findNearestSegmentIndex is used instead of + // NOTE: autoware_motion_utils::findNearestSegmentIndex is used instead of // trajectory_utils::findEgoSegmentIndex // for the case where input_traj_points is much longer than output_traj_points, and the // former has a stop point but the latter will not have. - const auto stop_seg_idx = motion_utils::findNearestSegmentIndex( + const auto stop_seg_idx = autoware_motion_utils::findNearestSegmentIndex( output_traj_points, input_stop_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); @@ -305,10 +307,10 @@ void ElasticBandSmoother::applyInputVelocity( } if (*stop_seg_idx == output_traj_points.size() - 2) { const double signed_projected_length_to_segment = - motion_utils::calcLongitudinalOffsetToSegment( + autoware_motion_utils::calcLongitudinalOffsetToSegment( output_traj_points, *stop_seg_idx, input_stop_pose.position); - const double segment_length = - motion_utils::calcSignedArcLength(output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); + const double segment_length = autoware_motion_utils::calcSignedArcLength( + output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); if (segment_length < signed_projected_length_to_segment) { // NOTE: input_stop_pose is outside output_traj_points. return false; diff --git a/planning/autoware_path_smoother/src/replan_checker.cpp b/planning/autoware_path_smoother/src/replan_checker.cpp index d9f7f4fb6ca1..67096d29cab8 100644 --- a/planning/autoware_path_smoother/src/replan_checker.cpp +++ b/planning/autoware_path_smoother/src/replan_checker.cpp @@ -14,10 +14,10 @@ #include "autoware/path_smoother/replan_checker.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_smoother/utils/trajectory_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include @@ -140,14 +140,14 @@ bool ReplanChecker::isPathAroundEgoChanged( // calculate ego's lateral offset to previous trajectory points const auto prev_ego_seg_idx = trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); - const double prev_ego_lat_offset = - motion_utils::calcLateralOffset(prev_traj_points, p.ego_pose.position, prev_ego_seg_idx); + const double prev_ego_lat_offset = autoware_motion_utils::calcLateralOffset( + prev_traj_points, p.ego_pose.position, prev_ego_seg_idx); // calculate ego's lateral offset to current trajectory points const auto ego_seg_idx = trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); const double ego_lat_offset = - motion_utils::calcLateralOffset(p.traj_points, p.ego_pose.position, ego_seg_idx); + autoware_motion_utils::calcLateralOffset(p.traj_points, p.ego_pose.position, ego_seg_idx); const double diff_ego_lat_offset = prev_ego_lat_offset - ego_lat_offset; if (std::abs(diff_ego_lat_offset) < max_path_shape_around_ego_lat_dist_) { @@ -170,17 +170,17 @@ bool ReplanChecker::isPathForwardChanged( constexpr double lon_dist_interval = 10.0; for (double lon_dist = lon_dist_interval; lon_dist <= max_path_shape_forward_lon_dist_; lon_dist += lon_dist_interval) { - const auto prev_forward_point = - motion_utils::calcLongitudinalOffsetPoint(prev_traj_points, prev_ego_seg_idx, lon_dist); + const auto prev_forward_point = autoware_motion_utils::calcLongitudinalOffsetPoint( + prev_traj_points, prev_ego_seg_idx, lon_dist); if (!prev_forward_point) { continue; } // calculate lateral offset of current trajectory points to prev forward point const auto forward_seg_idx = - motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + autoware_motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); const double forward_lat_offset = - motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); + autoware_motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); if (max_path_shape_forward_lat_dist_ < std::abs(forward_lat_offset)) { return true; } diff --git a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp index 4599f5c2055a..b55dc901b0cd 100644 --- a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/path_smoother/utils/trajectory_utils.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/path_smoother/utils/geometry_utils.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -54,10 +54,10 @@ std::vector resampleTrajectoryPoints( { constexpr bool enable_resampling_stop_point = true; - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory( + const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware_motion_utils::resampleTrajectory( traj, interval, false, true, true, enable_resampling_stop_point); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware_motion_utils::convertToTrajectoryPointArray(resampled_traj); } // NOTE: stop point will not be resampled @@ -66,17 +66,17 @@ std::vector resampleTrajectoryPointsWithoutStopPoint( { constexpr bool enable_resampling_stop_point = false; - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory( + const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware_motion_utils::resampleTrajectory( traj, interval, false, true, true, enable_resampling_stop_point); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware_motion_utils::convertToTrajectoryPointArray(resampled_traj); } void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx) { - const double offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + const double offset_to_segment = autoware_motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); const auto traj_spline = SplineInterpolationPoints2d(traj_points); diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index 0dac02e92e61..416528257fe3 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -15,6 +15,7 @@ autoware_control_msgs autoware_map_msgs + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -25,7 +26,6 @@ component_interface_utils lanelet2_extension lanelet2_io - motion_utils nav_msgs rclcpp tf2_msgs diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 5002b8fa08fc..2907d264d1a1 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include #include @@ -296,7 +296,7 @@ void PlanningInterfaceTestManager::publishNominalPath( { autoware::test_utils::publishToTargetNode( test_node_, target_node, topic_name, normal_path_pub_, - motion_utils::convertToPath( + autoware_motion_utils::convertToPath( autoware::test_utils::loadPathWithLaneIdInYaml()), 5); } diff --git a/planning/autoware_planning_topic_converter/package.xml b/planning/autoware_planning_topic_converter/package.xml index c6d7c08ca2a0..43009b2921e1 100644 --- a/planning/autoware_planning_topic_converter/package.xml +++ b/planning/autoware_planning_topic_converter/package.xml @@ -14,9 +14,9 @@ ament_cmake_auto + autoware_motion_utils autoware_planning_msgs autoware_universe_utils - motion_utils rclcpp rclcpp_components diff --git a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp index b9e260205632..a03897ebb86f 100644 --- a/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp @@ -14,8 +14,8 @@ #include "autoware/planning_topic_converter/path_to_trajectory.hpp" +#include #include -#include namespace autoware::planning_topic_converter { @@ -50,7 +50,7 @@ PathToTrajectory::PathToTrajectory(const rclcpp::NodeOptions & options) void PathToTrajectory::process(const Path::ConstSharedPtr msg) { const auto trajectory_points = convertToTrajectoryPoints(msg->points); - const auto output = motion_utils::convertToTrajectory(trajectory_points, msg->header); + const auto output = autoware_motion_utils::convertToTrajectory(trajectory_points, msg->header); pub_->publish(output); } diff --git a/planning/autoware_planning_validator/package.xml b/planning/autoware_planning_validator/package.xml index 3bd1574b5a38..f19caabaeb3f 100644 --- a/planning/autoware_planning_validator/package.xml +++ b/planning/autoware_planning_validator/package.xml @@ -15,13 +15,13 @@ autoware_cmake rosidl_default_generators + autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils diagnostic_updater geometry_msgs - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/planning/autoware_planning_validator/src/debug_marker.cpp b/planning/autoware_planning_validator/src/debug_marker.cpp index 0defa72684c9..d4aa27ca2ee9 100644 --- a/planning/autoware_planning_validator/src/debug_marker.cpp +++ b/planning/autoware_planning_validator/src/debug_marker.cpp @@ -14,8 +14,8 @@ #include "autoware/planning_validator/debug_marker.hpp" +#include #include -#include #include #include @@ -90,7 +90,7 @@ void PlanningValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs: { const auto now = node_->get_clock()->now(); const auto stop_wall_marker = - motion_utils::createStopVirtualWallMarker(pose, "autoware_planning_validator", now, 0); + autoware_motion_utils::createStopVirtualWallMarker(pose, "autoware_planning_validator", now, 0); autoware_universe_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp index 1e095e2fa424..b3f2ab88a4a4 100644 --- a/planning/autoware_planning_validator/src/planning_validator.cpp +++ b/planning/autoware_planning_validator/src/planning_validator.cpp @@ -16,8 +16,8 @@ #include "autoware/planning_validator/utils.hpp" +#include #include -#include #include #include @@ -450,7 +450,7 @@ bool PlanningValidator::checkValidSteeringRate(const Trajectory & trajectory) bool PlanningValidator::checkValidVelocityDeviation(const Trajectory & trajectory) { // TODO(horibe): set appropriate thresholds for index search - const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, current_kinematics_->pose.pose); validation_status_.velocity_deviation = std::abs( @@ -466,7 +466,7 @@ bool PlanningValidator::checkValidVelocityDeviation(const Trajectory & trajector bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajectory) { // TODO(horibe): set appropriate thresholds for index search - const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( trajectory.points, current_kinematics_->pose.pose); validation_status_.distance_deviation = autoware_universe_utils::calcDistance2d( @@ -487,7 +487,7 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory const auto ego_pose = current_kinematics_->pose.pose; const size_t idx = - motion_utils::findFirstNearestIndexWithSoftConstraints(trajectory.points, ego_pose); + autoware_motion_utils::findFirstNearestIndexWithSoftConstraints(trajectory.points, ego_pose); if (0 < idx && idx < trajectory.points.size() - 1) { return true; // ego-nearest point exists between trajectory points. @@ -495,8 +495,8 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory // Check if the valid longitudinal deviation for given segment index const auto HasValidLongitudinalDeviation = [&](const size_t seg_idx, const bool is_last) { - auto long_offset = - motion_utils::calcLongitudinalOffsetToSegment(trajectory.points, seg_idx, ego_pose.position); + auto long_offset = autoware_motion_utils::calcLongitudinalOffsetToSegment( + trajectory.points, seg_idx, ego_pose.position); // for last, need to remove distance for the last segment. if (is_last) { @@ -532,7 +532,7 @@ bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & tra return true; // Ego is almost stopped. } - const auto forward_length = motion_utils::calcSignedArcLength( + const auto forward_length = autoware_motion_utils::calcSignedArcLength( trajectory.points, current_kinematics_->pose.pose.position, trajectory.points.size() - 1); const auto acc = validation_params_.forward_trajectory_length_acceleration; diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp index 1f64e7c4ad07..3d21c89704a0 100644 --- a/planning/autoware_planning_validator/src/utils.cpp +++ b/planning/autoware_planning_validator/src/utils.cpp @@ -14,8 +14,8 @@ #include "autoware/planning_validator/utils.hpp" +#include #include -#include #include #include diff --git a/planning/autoware_remaining_distance_time_calculator/package.xml b/planning/autoware_remaining_distance_time_calculator/package.xml index 494c0a58e1e0..4e8fc23ffe39 100644 --- a/planning/autoware_remaining_distance_time_calculator/package.xml +++ b/planning/autoware_remaining_distance_time_calculator/package.xml @@ -12,13 +12,13 @@ autoware_cmake autoware_internal_msgs + autoware_motion_utils autoware_planning_msgs autoware_route_handler autoware_test_utils autoware_universe_utils geometry_msgs lanelet2_extension - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 684d146c141b..1362f04e720e 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -18,6 +18,7 @@ autoware_behavior_path_planner_common autoware_map_msgs autoware_mission_planner + autoware_motion_utils autoware_path_optimizer autoware_path_smoother autoware_perception_msgs @@ -32,7 +33,6 @@ lanelet2_extension map_loader map_projection_loader - motion_utils osqp_interface rclcpp rclcpp_components 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 40dc817d2d67..7bba3a6d5a72 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,11 +14,11 @@ #include "centerline_source/optimization_trajectory_based_centerline.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/path_optimizer/node.hpp" #include "autoware/path_smoother/elastic_band_smoother.hpp" #include "autoware/universe_utils/ros/parameter.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "static_centerline_generator_node.hpp" #include "utils.hpp" @@ -95,7 +95,8 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); + return autoware_motion_utils::resamplePath( + non_resampled_path_with_lane_id, behavior_path_interval); }(); pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); RCLCPP_INFO(node.get_logger(), "Calculated raw path with lane id and published."); @@ -103,7 +104,7 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( // convert path with lane id to path const auto raw_path = [&]() { const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); + return autoware_motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); }(); pub_raw_path_->publish(raw_path); RCLCPP_INFO(node.get_logger(), "Converted to path and published."); @@ -123,7 +124,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // convert to trajectory points const auto raw_traj_points = [&]() { const auto raw_traj = convert_to_trajectory(raw_path); - return motion_utils::convertToTrajectoryPointArray(raw_traj); + return autoware_motion_utils::convertToTrajectoryPointArray(raw_traj); }(); // create an instance of elastic band and model predictive trajectory. diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index b67e7cbc0a68..706f0e9c7547 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -14,6 +14,8 @@ #include "static_centerline_generator_node.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/parameter.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" @@ -24,8 +26,6 @@ #include "map_loader/lanelet2_map_loader_node.hpp" #include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include "map_projection_loader/map_projection_loader.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "type_alias.hpp" #include "utils.hpp" @@ -166,9 +166,10 @@ std::vector resample_trajectory_points( const std::vector & input_traj_points, const double resample_interval) { // resample and calculate trajectory points' orientation - const auto input_traj = motion_utils::convertToTrajectory(input_traj_points); - auto resampled_input_traj = motion_utils::resampleTrajectory(input_traj, resample_interval); - return motion_utils::convertToTrajectoryPointArray(resampled_input_traj); + const auto input_traj = autoware_motion_utils::convertToTrajectory(input_traj_points); + auto resampled_input_traj = + autoware_motion_utils::resampleTrajectory(input_traj, resample_interval); + return autoware_motion_utils::convertToTrajectoryPointArray(resampled_input_traj); } } // namespace @@ -273,7 +274,7 @@ void StaticCenterlineGeneratorNode::update_centerline_range( centerline.begin() + traj_range_indices_.second + 1); pub_centerline_->publish( - motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); + autoware_motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); } void StaticCenterlineGeneratorNode::run() @@ -325,10 +326,10 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout centerline_with_route.centerline = resample_trajectory_points(centerline_with_route.centerline, output_trajectory_interval); - pub_whole_centerline_->publish(motion_utils::convertToTrajectory( + pub_whole_centerline_->publish(autoware_motion_utils::convertToTrajectory( centerline_with_route.centerline, create_header(this->now()))); - pub_centerline_->publish(motion_utils::convertToTrajectory( + pub_centerline_->publish(autoware_motion_utils::convertToTrajectory( centerline_with_route.centerline, create_header(this->now()))); return centerline_with_route; diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 198461533377..fb5ea169b9c8 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -15,6 +15,7 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_universe_utils @@ -22,7 +23,6 @@ diagnostic_msgs eigen libpcl-all-dev - motion_utils pcl_conversions rclcpp rclcpp_components diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index 0559d39c7073..e4eaf259e95f 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -14,9 +14,9 @@ #include "debug_marker.hpp" +#include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index 798470fe4568..8a21f6cee6a3 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -19,8 +19,8 @@ #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "debug_marker.hpp" +#include #include -#include #include #include @@ -47,9 +47,9 @@ namespace autoware::surround_obstacle_checker { using autoware::vehicle_info_utils::VehicleInfo; +using autoware_motion_utils::VehicleStopChecker; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; -using motion_utils::VehicleStopChecker; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 6f6ed564d5ea..b64f681be46f 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -15,6 +15,8 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__NODE_HPP_ +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" @@ -28,8 +30,6 @@ #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 "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" diff --git a/planning/autoware_velocity_smoother/include/autoware/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 index c110d8fc038b..42d5a520c9e4 100644 --- a/planning/autoware_velocity_smoother/include/autoware/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 @@ -15,9 +15,9 @@ #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/motion_utils/trajectory/trajectory.hpp" #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 "rclcpp/rclcpp.hpp" #include "tf2/utils.h" diff --git a/planning/autoware_velocity_smoother/include/autoware/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 index 239c04a239f0..541ba62e95d7 100644 --- a/planning/autoware_velocity_smoother/include/autoware/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 @@ -15,7 +15,7 @@ #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 "autoware/motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp index e8b866af673d..14d0d8ab7ff8 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "osqp_interface/osqp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 1dd79054b975..1e2918d8e313 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "osqp_interface/osqp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 4651b95c5448..b88cd26eb446 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "osqp_interface/osqp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 1a8d93fb2974..997f7b16e565 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -20,13 +20,13 @@ autoware_cmake eigen3_cmake_module + autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation - motion_utils nav_msgs osqp_interface rclcpp diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index c80ea2a67ba7..42c5f1657999 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -14,11 +14,11 @@ #include "autoware/velocity_smoother/node.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/universe_utils/ros/update_param.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 @@ -300,7 +300,7 @@ void VelocitySmootherNode::initCommonParam() void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const { - Trajectory publishing_trajectory = motion_utils::convertToTrajectory(trajectory); + Trajectory publishing_trajectory = autoware_motion_utils::convertToTrajectory(trajectory); publishing_trajectory.header = base_traj_raw_ptr_->header; pub_trajectory_->publish(publishing_trajectory); published_time_publisher_->publish_if_subscribed( @@ -434,10 +434,10 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr } // calculate trajectory velocity - auto input_points = motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_); + auto input_points = autoware_motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_); // guard for invalid trajectory - input_points = motion_utils::removeOverlapPoints(input_points); + input_points = autoware_motion_utils::removeOverlapPoints(input_points); if (input_points.size() < 2) { RCLCPP_ERROR(get_logger(), "No enough points in trajectory after overlap points removal"); return; @@ -687,7 +687,7 @@ void VelocitySmootherNode::insertBehindVelocity( // TODO(planning/control team) deal with overlapped lanes with the same direction const size_t seg_idx = [&]() { // with distance and yaw thresholds - const auto opt_nearest_seg_idx = motion_utils::findNearestSegmentIndex( + const auto opt_nearest_seg_idx = autoware_motion_utils::findNearestSegmentIndex( prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); if (opt_nearest_seg_idx) { @@ -695,13 +695,14 @@ void VelocitySmootherNode::insertBehindVelocity( } // with distance threshold - const auto opt_second_nearest_seg_idx = motion_utils::findNearestSegmentIndex( + const auto opt_second_nearest_seg_idx = autoware_motion_utils::findNearestSegmentIndex( prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold); if (opt_second_nearest_seg_idx) { return opt_second_nearest_seg_idx.value(); } - return motion_utils::findNearestSegmentIndex(prev_output_, output.at(i).pose.position); + return autoware_motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose.position); }(); const auto prev_output_point = trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose, seg_idx); @@ -721,9 +722,9 @@ void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajecto // stop distance calculation const double stop_dist_lim{50.0}; double stop_dist{stop_dist_lim}; - const auto stop_idx{motion_utils::searchZeroVelocityIndex(trajectory)}; + const auto stop_idx{autoware_motion_utils::searchZeroVelocityIndex(trajectory)}; if (stop_idx) { - stop_dist = motion_utils::calcSignedArcLength(trajectory, closest, *stop_idx); + stop_dist = autoware_motion_utils::calcSignedArcLength(trajectory, closest, *stop_idx); } else { stop_dist = closest > 0 ? stop_dist : -stop_dist; } @@ -819,14 +820,14 @@ std::pair VelocitySmootherNode::ca void VelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { - const auto stop_idx = motion_utils::searchZeroVelocityIndex(input); + const auto stop_idx = autoware_motion_utils::searchZeroVelocityIndex(input); if (!stop_idx) { return; } // Get Closest Point from Output // TODO(planning/control team) deal with overlapped lanes with the same directions - const auto nearest_output_point_idx = motion_utils::findNearestIndex( + const auto nearest_output_point_idx = autoware_motion_utils::findNearestIndex( output, input.at(*stop_idx).pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); @@ -874,11 +875,12 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c // insert the point at the distance of external velocity limit const auto & current_pose = current_odometry_ptr_->pose.pose; - const size_t closest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - traj, current_pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); + const size_t closest_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj, current_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); const auto inserted_index = - motion_utils::insertTargetPoint(closest_seg_idx, external_velocity_limit_.dist, traj); + autoware_motion_utils::insertTargetPoint(closest_seg_idx, external_velocity_limit_.dist, traj); if (!inserted_index) { traj.back().longitudinal_velocity_mps = std::min( traj.back().longitudinal_velocity_mps, static_cast(external_velocity_limit_.velocity)); @@ -891,7 +893,7 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c // create virtual wall if (std::abs(external_velocity_limit_.velocity) < 1e-3) { - const auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker( + const auto virtual_wall_marker = autoware_motion_utils::createStopVirtualWallMarker( traj.at(*inserted_index).pose, external_velocity_limit_.sender, this->now(), 0, base_link2front_); pub_virtual_wall_->publish(virtual_wall_marker); @@ -903,7 +905,7 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { - const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj); + const auto stop_idx = autoware_motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { return; // no stop point. } @@ -1037,14 +1039,14 @@ bool VelocitySmootherNode::isEngageStatus(const double target_vel) const Trajectory VelocitySmootherNode::toTrajectoryMsg( const TrajectoryPoints & points, const std_msgs::msg::Header * header) const { - auto trajectory = motion_utils::convertToTrajectory(points); + auto trajectory = autoware_motion_utils::convertToTrajectory(points); trajectory.header = header ? *header : base_traj_raw_ptr_->header; return trajectory; } size_t VelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const { - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( points, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); } @@ -1074,9 +1076,10 @@ void VelocitySmootherNode::publishStopWatchTime() TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPoint( const TrajectoryPoints & trajectory, const Pose & pose) const { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory, pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); return trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, pose, current_seg_idx); } diff --git a/planning/autoware_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp index 00934f6648d7..f24dc17df053 100644 --- a/planning/autoware_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -14,10 +14,10 @@ #include "autoware/velocity_smoother/resample.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.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 #include @@ -32,17 +32,18 @@ TrajectoryPoints resampleTrajectory( const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v) { // Arc length from the initial point to the closest point - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double negative_front_arclength_value = motion_utils::calcSignedArcLength( + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = autoware_motion_utils::calcSignedArcLength( input, current_pose.position, current_seg_idx, input.at(0).pose.position, 0); const auto front_arclength_value = std::fabs(negative_front_arclength_value); const auto dist_to_closest_stop_point = - motion_utils::calcDistanceToForwardStopPoint(input, current_pose); + autoware_motion_utils::calcDistanceToForwardStopPoint(input, current_pose); // Get the resample size from the closest point - const double trajectory_length = motion_utils::calcArcLength(input); + const double trajectory_length = autoware_motion_utils::calcArcLength(input); const double Nt = param.resample_time / std::max(param.dense_resample_dt, 0.001); const double ds_nominal = std::max(v_current * param.dense_resample_dt, param.dense_min_interval_distance); @@ -127,9 +128,9 @@ TrajectoryPoints resampleTrajectory( return input; } - const auto output_traj = motion_utils::resampleTrajectory( - motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); + const auto output_traj = autoware_motion_utils::resampleTrajectory( + autoware_motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); + auto output = autoware_motion_utils::convertToTrajectoryPointArray(output_traj); // add end point directly to consider the endpoint velocity. if (is_endpoint_included) { @@ -150,9 +151,9 @@ TrajectoryPoints resampleTrajectory( const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v) { // input arclength - const double trajectory_length = motion_utils::calcArcLength(input); + const double trajectory_length = autoware_motion_utils::calcArcLength(input); const auto dist_to_closest_stop_point = - motion_utils::calcDistanceToForwardStopPoint(input, current_pose); + autoware_motion_utils::calcDistanceToForwardStopPoint(input, current_pose); // distance to stop point double stop_arclength_value = param.max_trajectory_length; @@ -170,9 +171,10 @@ TrajectoryPoints resampleTrajectory( // Step1. Resample front trajectory // Arc length from the initial point to the closest point - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double negative_front_arclength_value = motion_utils::calcSignedArcLength( + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = autoware_motion_utils::calcSignedArcLength( input, current_pose.position, current_seg_idx, input.at(0).pose.position, static_cast(0)); const auto front_arclength_value = std::fabs(negative_front_arclength_value); @@ -246,9 +248,9 @@ TrajectoryPoints resampleTrajectory( return input; } - const auto output_traj = motion_utils::resampleTrajectory( - motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); + const auto output_traj = autoware_motion_utils::resampleTrajectory( + autoware_motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v); + auto output = autoware_motion_utils::convertToTrajectoryPointArray(output_traj); // add end point directly to consider the endpoint velocity. if (is_endpoint_included) { diff --git a/planning/autoware_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 index 1124fedbdd28..27b54ccf8bf2 100644 --- a/planning/autoware_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 @@ -14,9 +14,9 @@ #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include #include @@ -298,9 +298,9 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilt for (double s = 0; s < in_arclength.back(); s += points_interval) { out_arclength.push_back(s); } - const auto output_traj = - motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - output = motion_utils::convertToTrajectoryPointArray(output_traj); + const auto output_traj = autoware_motion_utils::resampleTrajectory( + autoware_motion_utils::convertToTrajectory(input), out_arclength); + output = autoware_motion_utils::convertToTrajectoryPointArray(output_traj); output.back() = input.back(); // keep the final speed. } else { output = input; diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 3761d16d7936..9f9f50b95575 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -131,7 +131,7 @@ bool JerkFilteredSmoother::apply( // to avoid getting 0 as a stop point, search zero velocity index from 1. // the size of the resampled trajectory must not be less than 2. - const auto zero_vel_id = motion_utils::searchZeroVelocityIndex( + const auto zero_vel_id = autoware_motion_utils::searchZeroVelocityIndex( opt_resampled_trajectory, 1, opt_resampled_trajectory.size()); if (!zero_vel_id) { diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index d0e6ffa94df2..a1972d5dbc1e 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -14,13 +14,13 @@ #include "autoware/velocity_smoother/smoother/smoother_base.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.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 #include @@ -34,10 +34,10 @@ namespace TrajectoryPoints applyPreProcess( const TrajectoryPoints & input, const double interval, const bool use_resampling) { - using motion_utils::calcArcLength; - using motion_utils::convertToTrajectory; - using motion_utils::convertToTrajectoryPointArray; - using motion_utils::resampleTrajectory; + using autoware_motion_utils::calcArcLength; + using autoware_motion_utils::convertToTrajectory; + using autoware_motion_utils::convertToTrajectoryPointArray; + using autoware_motion_utils::resampleTrajectory; if (!use_resampling) { return input; @@ -141,13 +141,13 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( // since the resampling takes a long time, omit the resampling when it is not requested if (use_resampling) { std::vector out_arclength; - const auto traj_length = motion_utils::calcArcLength(input); + const auto traj_length = autoware_motion_utils::calcArcLength(input); for (double s = 0; s < traj_length; s += points_interval) { out_arclength.push_back(s); } - const auto output_traj = - motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - output = motion_utils::convertToTrajectoryPointArray(output_traj); + const auto output_traj = autoware_motion_utils::resampleTrajectory( + autoware_motion_utils::convertToTrajectory(input), out_arclength); + output = autoware_motion_utils::convertToTrajectoryPointArray(output_traj); output.back() = input.back(); // keep the final speed. } else { output = input; diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index 35ba7d3972fd..51a2be57ebe0 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/linear_interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -596,7 +596,7 @@ std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest) { - const auto idx = motion_utils::searchZeroVelocityIndex(trajectory); + const auto idx = autoware_motion_utils::searchZeroVelocityIndex(trajectory); if (!idx) { return std::numeric_limits::max(); // stop point is located far away diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml index a4561b1c960d..e9a4ba509431 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml @@ -22,10 +22,10 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_behavior_path_static_obstacle_avoidance_module + autoware_motion_utils autoware_rtc_interface autoware_universe_utils lanelet2_extension - motion_utils pluginlib rclcpp tier4_planning_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 769154ec9e10..247f94c63fc8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -195,10 +195,10 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( std::optional AvoidanceByLaneChange::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { + using autoware_motion_utils::findNearestIndex; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::calcLateralDeviation; using boost::geometry::return_centroid; - using motion_utils::findNearestIndex; const auto p = std::dynamic_pointer_cast(avoidance_parameters_); diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 9ec4b0c71650..6dd430ce35f4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -18,6 +18,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_universe_utils @@ -25,7 +26,6 @@ geometry_msgs lanelet2_core lanelet2_extension - motion_utils object_recognition_utils pluginlib rclcpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index 2643166350a1..0c8ec1c530b7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -117,7 +117,7 @@ std::pair projectObstacleVelocityToTrajectory( { const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; const double obj_yaw = tf2::getYaw(obj_pose.orientation); - const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); + const size_t obj_idx = autoware_motion_utils::findNearestIndex(path_points, obj_pose.position); const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); const Eigen::Rotation2Dd R_ego_to_obstacle(obj_yaw - path_yaw); @@ -172,7 +172,8 @@ double calcDiffAngleAgainstPath( const std::vector & path_points, const geometry_msgs::msg::Pose & target_pose) { - const size_t nearest_idx = motion_utils::findNearestIndex(path_points, target_pose.position); + const size_t nearest_idx = + autoware_motion_utils::findNearestIndex(path_points, target_pose.position); const double traj_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); const double target_yaw = tf2::getYaw(target_pose.orientation); @@ -184,8 +185,8 @@ double calcDiffAngleAgainstPath( [[maybe_unused]] double calcDiffAngleBetweenPaths( const std::vector & path_points, const PredictedPath & predicted_path) { - const size_t nearest_idx = - motion_utils::findNearestSegmentIndex(path_points, predicted_path.path.front().position); + const size_t nearest_idx = autoware_motion_utils::findNearestSegmentIndex( + path_points, predicted_path.path.front().position); const double ego_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); constexpr size_t max_predicted_path_size = 5; @@ -204,7 +205,7 @@ double calcDistanceToPath( const std::vector & path_points, const geometry_msgs::msg::Point & target_pos) { - const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); + const size_t target_idx = autoware_motion_utils::findNearestIndex(path_points, target_pos); if (target_idx == 0 || target_idx == path_points.size() - 1) { const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); const double angle_to_target_pos = autoware_universe_utils::calcAzimuthAngle( @@ -219,14 +220,14 @@ double calcDistanceToPath( } } - return std::abs(motion_utils::calcLateralOffset(path_points, target_pos)); + return std::abs(autoware_motion_utils::calcLateralOffset(path_points, target_pos)); } bool isLeft( const std::vector & path_points, const geometry_msgs::msg::Point & target_pos) { - const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); + const size_t target_idx = autoware_motion_utils::findNearestIndex(path_points, target_pos); const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); const double angle_to_target_pos = autoware_universe_utils::calcAzimuthAngle( path_points.at(target_idx).point.pose.position, target_pos); @@ -328,7 +329,7 @@ bool DynamicObstacleAvoidanceModule::isExecutionRequested() const } // check if the ego is driving forward - const auto is_driving_forward = motion_utils::isDrivingForward(input_path.points); + const auto is_driving_forward = autoware_motion_utils::isDrivingForward(input_path.points); if (!is_driving_forward || !(*is_driving_forward)) { return false; } @@ -625,7 +626,7 @@ void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( // 1.f. calculate the object is on ego's path or not const double dist_obj_center_to_path = - std::abs(motion_utils::calcLateralOffset(input_path.points, obj_pose.position)); + std::abs(autoware_motion_utils::calcLateralOffset(input_path.points, obj_pose.position)); const bool is_object_on_ego_path = dist_obj_center_to_path < planner_data_->parameters.vehicle_width / 2.0 + parameters_->min_obj_lat_offset_to_ego_path; @@ -765,7 +766,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje // 2.g. check if the ego is not ahead of the object. const double signed_dist_ego_to_obj = [&]() { const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); - const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( + const double lon_offset_ego_to_obj = autoware_motion_utils::calcSignedArcLength( input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( @@ -830,7 +831,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedOb getLateralLongitudinalOffset(input_path.points, object.pose, object.shape); const double signed_dist_ego_to_obj = [&]() { const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); - const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( + const double lon_offset_ego_to_obj = autoware_motion_utils::calcSignedArcLength( input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( @@ -917,7 +918,7 @@ LatFeasiblePaths DynamicObstacleAvoidanceModule::generateLateralFeasiblePaths( // check if the ego is close enough to the current ref path, meaning that lane change ends. const auto ego_pos = getEgoPose().position; const double dist_to_ref_path = - std::abs(motion_utils::calcLateralOffset(ego_ref_path_points, ego_pos)); + std::abs(autoware_motion_utils::calcLateralOffset(ego_ref_path_points, ego_pos)); constexpr double epsilon_dist_to_ref_path = 0.5; if (dist_to_ref_path < epsilon_dist_to_ref_path) { @@ -926,7 +927,7 @@ LatFeasiblePaths DynamicObstacleAvoidanceModule::generateLateralFeasiblePaths( } else { // check if the ego is during lane change. if (prev_input_ref_path_points_ && !prev_input_ref_path_points_->empty()) { - const double dist_ref_paths = std::abs(motion_utils::calcLateralOffset( + const double dist_ref_paths = std::abs(autoware_motion_utils::calcLateralOffset( ego_ref_path_points, prev_input_ref_path_points_->front().point.pose.position)); constexpr double epsilon_ref_paths_diff = 1.0; if (epsilon_ref_paths_diff < dist_ref_paths) { @@ -981,7 +982,7 @@ TimeWhileCollision DynamicObstacleAvoidanceModule::calcTimeWhileCollision( // Set maximum time-to-collision 0 if the object longitudinally overlaps ego. // NOTE: This is to avoid objects running right beside ego even if time-to-collision is negative. const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); - const double lon_offset_ego_to_obj_idx = motion_utils::calcSignedArcLength( + const double lon_offset_ego_to_obj_idx = autoware_motion_utils::calcSignedArcLength( ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); const double relative_velocity = getEgoSpeed() - obj_tangent_vel; @@ -1053,7 +1054,7 @@ bool DynamicObstacleAvoidanceModule::willObjectCutIn( const bool will_object_cut_in = [&]() { for (const auto & predicted_path_point : predicted_path.path) { const double paths_lat_diff = - motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); + autoware_motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); if (std::abs(paths_lat_diff) < planner_data_->parameters.vehicle_width / 2.0) { return true; } @@ -1069,7 +1070,7 @@ bool DynamicObstacleAvoidanceModule::willObjectCutIn( const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); const double relative_velocity = getEgoSpeed() - obj_tangent_vel; const double lon_offset_ego_to_obj = - motion_utils::calcSignedArcLength( + autoware_motion_utils::calcSignedArcLength( ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) + lat_lon_offset.min_lon_offset; if ( @@ -1132,8 +1133,8 @@ DynamicObstacleAvoidanceModule::DecisionWithReason DynamicObstacleAvoidanceModul } // Check if object is in the lane before ego's lane change. - const double dist_to_ref_path_before_lane_change = - std::abs(motion_utils::calcLateralOffset(*ref_path_before_lane_change_, obj_pose.position)); + const double dist_to_ref_path_before_lane_change = std::abs( + autoware_motion_utils::calcLateralOffset(*ref_path_before_lane_change_, obj_pose.position)); const double epsilon_dist_checking_in_lane = calcObstacleWidth(obj_shape); if (epsilon_dist_checking_in_lane < dist_to_ref_path_before_lane_change) { return false; @@ -1188,7 +1189,8 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_perception_msgs::msg::Shape & obj_shape) const { - const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); + const size_t obj_seg_idx = + autoware_motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); const auto obj_points = autoware_universe_utils::toPolygon2d(obj_pose, obj_shape); // TODO(murooka) calculation is not so accurate. @@ -1197,16 +1199,16 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const size_t obj_point_seg_idx = - motion_utils::findNearestSegmentIndex(ego_path, geom_obj_point); + autoware_motion_utils::findNearestSegmentIndex(ego_path, geom_obj_point); // calculate lateral offset const double obj_point_lat_offset = - motion_utils::calcLateralOffset(ego_path, geom_obj_point, obj_point_seg_idx); + autoware_motion_utils::calcLateralOffset(ego_path, geom_obj_point, obj_point_seg_idx); obj_lat_offset_vec.push_back(obj_point_lat_offset); // calculate longitudinal offset const double lon_offset = - motion_utils::calcLongitudinalOffsetToSegment(ego_path, obj_seg_idx, geom_obj_point); + autoware_motion_utils::calcLongitudinalOffsetToSegment(ego_path, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } @@ -1225,14 +1227,14 @@ MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( const TimeWhileCollision & time_while_collision) const { const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, obj_pose.position); + autoware_motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, obj_pose.position); // calculate min/max longitudinal offset from object to path const auto obj_lon_offset = [&]() { std::vector obj_lon_offset_vec; for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); - const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( + const double lon_offset = autoware_motion_utils::calcLongitudinalOffsetToSegment( ref_path_points_for_obj_poly, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } @@ -1297,7 +1299,7 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( { const auto & input_path_points = getPreviousModuleOutput().path.points; const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); + autoware_motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); constexpr double dist_threshold_additional_margin = 0.5; const double dist_threshold_paths = @@ -1315,7 +1317,7 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( std::reverse(cropped_ego_path_points.begin(), cropped_ego_path_points.end()); } if (cropped_ego_path_points.size() < 2) { - return motion_utils::calcArcLength(obj_path.path); + return autoware_motion_utils::calcArcLength(obj_path.path); } // calculate where the object's path will be forked from (= far from) the ego's path. @@ -1375,11 +1377,12 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( const double partial_segment_length = segment_length * (dist_threshold_paths - *prev_min_dist) / (*next_min_dist - *prev_min_dist); - return motion_utils::calcSignedArcLength(obj_path.path, 0, prev_valid_obj_path_end_idx) + + return autoware_motion_utils::calcSignedArcLength( + obj_path.path, 0, prev_valid_obj_path_end_idx) + partial_segment_length; } } - return motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); + return autoware_motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); } // min value denotes near side, max value denotes far side @@ -1397,8 +1400,8 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( return true; } const size_t obj_point_idx = - motion_utils::findNearestIndex(ref_path_points_for_obj_poly, obj_pos); - const double paths_lat_diff = std::abs(motion_utils::calcLateralOffset( + autoware_motion_utils::findNearestIndex(ref_path_points_for_obj_poly, obj_pos); + const double paths_lat_diff = std::abs(autoware_motion_utils::calcLateralOffset( prev_object->ref_path_points_for_obj_poly, ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); @@ -1416,9 +1419,9 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( std::vector obj_lat_abs_offset_vec; for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); - const size_t obj_point_seg_idx = - motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, geom_obj_point); - const double obj_point_lat_offset = motion_utils::calcLateralOffset( + const size_t obj_point_seg_idx = autoware_motion_utils::findNearestSegmentIndex( + ref_path_points_for_obj_poly, geom_obj_point); + const double obj_point_lat_offset = autoware_motion_utils::calcLateralOffset( ref_path_points_for_obj_poly, geom_obj_point, obj_point_seg_idx); obj_lat_abs_offset_vec.push_back(obj_point_lat_offset); } @@ -1491,8 +1494,8 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( return true; } const size_t obj_point_idx = - motion_utils::findNearestIndex(ref_path_points_for_obj_poly, object.pose.position); - const double paths_lat_diff = std::abs(motion_utils::calcLateralOffset( + autoware_motion_utils::findNearestIndex(ref_path_points_for_obj_poly, object.pose.position); + const double paths_lat_diff = std::abs(autoware_motion_utils::calcLateralOffset( prev_object->ref_path_points_for_obj_poly, ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); @@ -1510,9 +1513,10 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( std::vector lat_pos_vec; for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); - const double obj_point_lat_offset = motion_utils::calcLateralOffset( + const double obj_point_lat_offset = autoware_motion_utils::calcLateralOffset( ref_path_points_for_obj_poly, geom_obj_point, - motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, geom_obj_point)); + autoware_motion_utils::findNearestSegmentIndex( + ref_path_points_for_obj_poly, geom_obj_point)); lat_pos_vec.push_back(obj_point_lat_offset); } const auto current_pos_area = getMinMaxValues(lat_pos_vec); @@ -1572,16 +1576,16 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( auto ref_path_points_for_obj_poly = object.ref_path_points_for_obj_poly; - const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, object.pose.position); + const size_t obj_seg_idx = autoware_motion_utils::findNearestSegmentIndex( + ref_path_points_for_obj_poly, object.pose.position); // const auto obj_points = autoware_universe_utils::toPolygon2d(object.pose, object.shape); - const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( + const auto lon_bound_start_idx_opt = autoware_motion_utils::insertTargetPoint( obj_seg_idx, object.lon_offset_to_avoid->min_value, ref_path_points_for_obj_poly); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; - const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( + const auto lon_bound_end_idx_opt = autoware_motion_utils::insertTargetPoint( updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, ref_path_points_for_obj_poly); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { @@ -1618,14 +1622,14 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( obj_inner_bound_poses.at(bound_seg_idx), intersect_point); const auto obj_inner_bound_start_idx_opt = - motion_utils::insertTargetPoint(bound_seg_idx, lon_offset, obj_inner_bound_poses); + autoware_motion_utils::insertTargetPoint(bound_seg_idx, lon_offset, obj_inner_bound_poses); if (obj_inner_bound_start_idx_opt) { return *obj_inner_bound_start_idx_opt; } } // Check if the object polygon is fully outside the ego_lat_feasible_path. - const double obj_poly_lat_offset = motion_utils::calcLateralOffset( + const double obj_poly_lat_offset = autoware_motion_utils::calcLateralOffset( ego_lat_feasible_path, obj_inner_bound_poses.front().position); if ( (!object.is_collision_left && 0 < obj_poly_lat_offset) || @@ -1679,7 +1683,7 @@ DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( // calculate left and right bound std::vector obj_left_bound_points; std::vector obj_right_bound_points; - const double obj_path_length = motion_utils::calcArcLength(obj_path.path); + const double obj_path_length = autoware_motion_utils::calcArcLength(obj_path.path); for (size_t i = 0; i < obj_path.path.size(); ++i) { const double lon_offset = [&]() { if (i == 0) diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml index 83ddcfe98906..41db5ade58a7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -21,9 +21,9 @@ autoware_behavior_path_lane_change_module autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_rtc_interface autoware_universe_utils - motion_utils pluginlib rclcpp tier4_planning_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 0097a6d6e0b4..ed2735448fdb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -31,8 +31,8 @@ #include #include #include +#include #include -#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp index 5cbe3bc4ee84..d1760c7f5a13 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -67,7 +67,7 @@ struct PullOverPath partial_paths.at(i).points.end()); } } - path.points = motion_utils::removeOverlapPoints(path.points); + path.points = autoware_motion_utils::removeOverlapPoints(path.points); return path; } @@ -75,7 +75,8 @@ struct PullOverPath PathWithLaneId getParkingPath() const { const PathWithLaneId full_path = getFullPath(); - const size_t start_idx = motion_utils::findNearestIndex(full_path.points, start_pose.position); + const size_t start_idx = + autoware_motion_utils::findNearestIndex(full_path.points, start_pose.position); PathWithLaneId parking_path{}; std::copy( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index 148a3f966303..2f107c5644f2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -22,9 +22,9 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_rtc_interface autoware_universe_utils - motion_utils pluginlib rclcpp tier4_planning_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp index 64d141e202b9..3082bcf26cba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -123,7 +123,7 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) // Check if driving forward for each path, return empty if not for (auto & path : partial_paths) { - if (!motion_utils::isDrivingForward(path.points)) { + if (!autoware_motion_utils::isDrivingForward(path.points)) { return {}; } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 79078e15d471..a329e984fc7c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -40,14 +40,14 @@ #include using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; +using autoware_motion_utils::calcLongitudinalOffsetPose; +using autoware_motion_utils::calcSignedArcLength; +using autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using autoware_motion_utils::insertDecelPoint; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::calcOffsetPose; using autoware_universe_utils::createMarkerColor; using autoware_universe_utils::inverseTransformPose; -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcSignedArcLength; -using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; -using motion_utils::insertDecelPoint; using nav_msgs::msg::OccupancyGrid; namespace autoware::behavior_path_planner @@ -137,7 +137,7 @@ bool GoalPlannerModule::hasPreviousModulePathShapeChanged( // the last path constexpr double LATERAL_DEVIATION_THRESH = 0.3; for (const auto & p : previous_module_output.path.points) { - const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex( + const size_t nearest_seg_idx = autoware_motion_utils::findNearestSegmentIndex( last_previous_module_output->path.points, p.point.pose.position); const auto seg_front = last_previous_module_output->path.points.at(nearest_seg_idx); const auto seg_back = last_previous_module_output->path.points.at(nearest_seg_idx + 1); @@ -155,7 +155,7 @@ bool GoalPlannerModule::hasPreviousModulePathShapeChanged( // p.point.pose.position is not within the segment, skip lateral distance check continue; } - const double lateral_distance = std::abs(motion_utils::calcLateralOffset( + const double lateral_distance = std::abs(autoware_motion_utils::calcLateralOffset( last_previous_module_output->path.points, p.point.pose.position, nearest_seg_idx)); if (lateral_distance > LATERAL_DEVIATION_THRESH) { return true; @@ -171,7 +171,7 @@ bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath( if (!last_previous_module_output) { return true; } - return std::abs(motion_utils::calcLateralOffset( + return std::abs(autoware_motion_utils::calcLateralOffset( last_previous_module_output->path.points, planner_data->self_odometry->pose.pose.position)) > 0.3; } @@ -181,7 +181,7 @@ bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath( const BehaviorModuleOutput & previous_module_output) const { constexpr double LATERAL_DEVIATION_THRESH = 0.3; - return std::abs(motion_utils::calcLateralOffset( + return std::abs(autoware_motion_utils::calcLateralOffset( previous_module_output.path.points, planner_data->self_odometry->pose.pose.position)) > LATERAL_DEVIATION_THRESH; } @@ -770,7 +770,7 @@ bool GoalPlannerModule::canReturnToLaneParking() const Point & current_point = planner_data_->self_odometry->pose.pose.position; constexpr double th_distance = 0.5; const bool is_close_to_path = - std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance; + std::abs(autoware_motion_utils::calcLateralOffset(path.points, current_point)) < th_distance; if (!is_close_to_path) { return false; } @@ -1266,10 +1266,10 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( // if ego is sufficiently close to the start of the nearest candidate path, the path is decided const auto & current_pose = planner_data->self_odometry->pose.pose; const size_t ego_segment_idx = - motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); + autoware_motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); - const size_t start_pose_segment_idx = - motion_utils::findNearestSegmentIndex(current_path.points, pull_over_path->start_pose.position); + const size_t start_pose_segment_idx = autoware_motion_utils::findNearestSegmentIndex( + current_path.points, pull_over_path->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( current_path.points, current_pose.position, ego_segment_idx, pull_over_path->start_pose.position, start_pose_segment_idx); @@ -1500,19 +1500,19 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); - const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( + const auto ego_segment_idx = autoware_motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), M_PI_2); if (!ego_segment_idx) { return {std::numeric_limits::max(), std::numeric_limits::max()}; } - const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( + const size_t start_pose_segment_idx = autoware_motion_utils::findNearestSegmentIndex( full_path.points, thread_safe_data_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); - const size_t goal_pose_segment_idx = motion_utils::findNearestSegmentIndex( + const size_t goal_pose_segment_idx = autoware_motion_utils::findNearestSegmentIndex( full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, @@ -1646,7 +1646,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId auto stop_path = path; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto stop_idx = - motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); + autoware_motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { debug_stop_pose_with_info_.set(stop_path.points.at(*stop_idx).point.pose, "feasible stop"); } @@ -1788,8 +1788,10 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose; const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; - const auto shift_start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - const auto shift_end_idx = motion_utils::findNearestIndex(path.points, end_pose.position); + const auto shift_start_idx = + autoware_motion_utils::findNearestIndex(path.points, start_pose.position); + const auto shift_end_idx = + autoware_motion_utils::findNearestIndex(path.points, end_pose.position); const auto is_ignore_signal = [this](const lanelet::Id & id) { if (!ignore_signal_.has_value()) { @@ -1832,8 +1834,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() constexpr double distance_threshold = 1.0; const auto stop_point = thread_safe_data_.get_pull_over_path()->partial_paths.front().points.back(); - const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength( - path.points, stop_point.point.pose.position, current_pose.position)); + const double distance_from_ego_to_stop_point = + std::abs(autoware_motion_utils::calcSignedArcLength( + path.points, stop_point.point.pose.position, current_pose.position)); return distance_from_ego_to_stop_point < distance_threshold; }); @@ -1886,7 +1889,7 @@ bool GoalPlannerModule::checkObjectsCollision( * - `extra_lateral_margin` adds the lateral margin on curves. */ std::vector ego_polygons_expanded{}; - const auto curvatures = motion_utils::calcCurvature(path.points); + const auto curvatures = autoware_motion_utils::calcCurvature(path.points); for (size_t i = 0; i < path.points.size(); ++i) { const auto p = path.points.at(i); @@ -2359,6 +2362,7 @@ void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); + using autoware_motion_utils::createStopVirtualWallMarker; using autoware_universe_utils::createDefaultMarker; using autoware_universe_utils::createMarkerColor; using autoware_universe_utils::createMarkerScale; @@ -2369,7 +2373,6 @@ void GoalPlannerModule::setDebugData() using marker_utils::showPolygon; using marker_utils::showPredictedPath; using marker_utils::showSafetyCheckInfo; - using motion_utils::createStopVirtualWallMarker; const auto header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 87d1034f2068..564d22f2df2e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -156,7 +156,7 @@ GoalCandidates GoalSearcher::search( // original means non lateral offset poses const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = - std::abs(motion_utils::calcSignedArcLength( + std::abs(autoware_motion_utils::calcSignedArcLength( center_line_path.points, reference_goal_pose_.position, original_search_pose.position)); original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index 8cd368106f8d..46b4c66a7c9f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -101,8 +101,8 @@ std::optional ShiftPullOver::cropPrevModulePath( const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const { // clip previous module path to shift end pose nearest segment index - const size_t shift_end_idx = - motion_utils::findNearestSegmentIndex(prev_module_path.points, shift_end_pose.position); + const size_t shift_end_idx = autoware_motion_utils::findNearestSegmentIndex( + prev_module_path.points, shift_end_pose.position); std::vector clipped_points{ prev_module_path.points.begin(), prev_module_path.points.begin() + shift_end_idx}; if (clipped_points.empty()) { @@ -111,7 +111,7 @@ std::optional ShiftPullOver::cropPrevModulePath( // add projected shift end pose to clipped points PathPointWithLaneId projected_point = clipped_points.back(); - const double offset = motion_utils::calcSignedArcLength( + const double offset = autoware_motion_utils::calcSignedArcLength( prev_module_path.points, shift_end_idx, shift_end_pose.position); projected_point.point.pose = autoware_universe_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); @@ -171,7 +171,7 @@ std::optional ShiftPullOver::generatePullOverPath( shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); - const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose( + const auto shift_start_pose = autoware_motion_utils::calcLongitudinalOffsetPose( processed_prev_module_path->points, shift_end_pose_prev_module_path.position, -before_shifted_pull_over_distance); @@ -188,8 +188,8 @@ std::optional ShiftPullOver::generatePullOverPath( if (!path_shifter.generate(&shifted_path, offset_back)) { return {}; } - shifted_path.path.points = motion_utils::removeOverlapPoints(shifted_path.path.points); - motion_utils::insertOrientation(shifted_path.path.points, true); + shifted_path.path.points = autoware_motion_utils::removeOverlapPoints(shifted_path.path.points); + autoware_motion_utils::insertOrientation(shifted_path.path.points, true); // set same orientation, because the reference center line orientation is not same to the shifted_path.path.points.back().point.pose.orientation = shift_end_pose.orientation; @@ -309,7 +309,7 @@ double ShiftPullOver::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; for (const auto & [k, segment_length] : - motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { + autoware_motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { // after shifted segment length const double after_segment_length = k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 81aa6c352fe4..f99a240f291a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -305,8 +305,8 @@ double calcLateralDeviationBetweenPaths( { double lateral_deviation = 0.0; for (const auto & target_point : target_path.points) { - const size_t nearest_index = - motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position); + const size_t nearest_index = autoware_motion_utils::findNearestIndex( + reference_path.points, target_point.point.pose.position); lateral_deviation = std::max( lateral_deviation, std::abs(autoware_universe_utils::calcLateralDeviation( @@ -325,7 +325,8 @@ bool isReferencePath( std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose) { - const size_t end_idx = motion_utils::findNearestSegmentIndex(path.points, end_pose.position); + const size_t end_idx = + autoware_motion_utils::findNearestSegmentIndex(path.points, end_pose.position); std::vector clipped_points{ path.points.begin(), path.points.begin() + end_idx}; if (clipped_points.empty()) { @@ -334,7 +335,8 @@ std::optional cropPath(const PathWithLaneId & path, const Pose & // add projected end pose to clipped points PathPointWithLaneId projected_point = clipped_points.back(); - const double offset = motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); + const double offset = + autoware_motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); projected_point.point.pose = autoware_universe_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); clipped_points.push_back(projected_point); @@ -391,15 +393,15 @@ PathWithLaneId extendPath( const auto & target_terminal_pose = target_path.points.back().point.pose; // generate clipped road lane reference path from previous module path terminal pose to shift end - const size_t target_path_terminal_idx = - motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); + const size_t target_path_terminal_idx = autoware_motion_utils::findNearestSegmentIndex( + reference_path.points, target_terminal_pose.position); PathWithLaneId clipped_path = cropForwardPoints(reference_path, target_path_terminal_idx, extend_length); // shift clipped path to previous module path terminal pose const double lateral_shift_from_reference_path = - motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); + autoware_motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); for (auto & p : clipped_path.points) { p.point.pose = autoware_universe_utils::calcOffsetPose( p.point.pose, 0, lateral_shift_from_reference_path, 0); @@ -417,7 +419,7 @@ PathWithLaneId extendPath( }); std::copy(start_point, clipped_path.points.end(), std::back_inserter(extended_path.points)); - extended_path.points = motion_utils::removeOverlapPoints(extended_path.points); + extended_path.points = autoware_motion_utils::removeOverlapPoints(extended_path.points); return extended_path; } @@ -427,9 +429,9 @@ PathWithLaneId extendPath( const Pose & extend_pose) { const auto & target_terminal_pose = target_path.points.back().point.pose; - const size_t target_path_terminal_idx = - motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); - const double extend_distance = motion_utils::calcSignedArcLength( + const size_t target_path_terminal_idx = autoware_motion_utils::findNearestSegmentIndex( + reference_path.points, target_terminal_pose.position); + const double extend_distance = autoware_motion_utils::calcSignedArcLength( reference_path.points, target_path_terminal_idx, extend_pose.position); return extendPath(target_path, reference_path, extend_distance); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index 8ea741f74ba1..74d11dbcb1e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -22,9 +22,9 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_rtc_interface autoware_universe_utils - motion_utils pluginlib rclcpp tier4_planning_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 1c0f12549b86..435769231e08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -352,9 +352,9 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto current_position = module_type_->getEgoPosition(); const auto status = module_type_->getLaneChangeStatus(); - const auto start_distance = motion_utils::calcSignedArcLength( + const auto start_distance = autoware_motion_utils::calcSignedArcLength( output.path.points, current_position, status.lane_change_path.info.shift_line.start.position); - const auto finish_distance = motion_utils::calcSignedArcLength( + const auto finish_distance = autoware_motion_utils::calcSignedArcLength( output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); steering_factor_interface_ptr_->updateSteeringFactor( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 83198b5328c8..f00c68c8651f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -37,7 +37,7 @@ namespace autoware::behavior_path_planner { -using motion_utils::calcSignedArcLength; +using autoware_motion_utils::calcSignedArcLength; using utils::lane_change::calcMinimumLaneChangeLength; using utils::lane_change::createLanesPolygon; using utils::path_safety_checker::isPolygonOverlapLanelet; @@ -182,13 +182,13 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const double buffer = next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; - const double path_length = motion_utils::calcArcLength(path.points); + const double path_length = autoware_motion_utils::calcArcLength(path.points); const size_t current_nearest_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); - const auto start_pose = - motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0)); + const auto start_pose = autoware_motion_utils::calcLongitudinalOffsetPose( + path.points, 0, std::max(path_length - buffer, 0.0)); if (dist_to_terminal - base_to_front < buffer && start_pose) { // modify turn signal current_turn_signal_info.desired_start_point = *start_pose; @@ -392,7 +392,7 @@ void NormalLaneChange::insertStopPoint( autoware_universe_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); for (const auto & polygon_p : polygon) { const auto p_fp = autoware_universe_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + const auto lateral_fp = autoware_motion_utils::calcLateralOffset(path.points, p_fp); // ignore if the point is around the ego path if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { @@ -690,7 +690,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } - const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const auto nearest_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -729,7 +729,7 @@ bool NormalLaneChange::isAbleToStopSafely() const return false; } - const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const auto nearest_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -798,7 +798,7 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons const auto vehicle_min_acc = std::max(p.min_acc, lane_change_parameters_->min_longitudinal_acc); const auto vehicle_max_acc = std::min(p.max_acc, lane_change_parameters_->max_longitudinal_acc); - const auto ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const auto ego_seg_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prev_module_output_.path.points, getEgoPose(), p.ego_nearest_dist_threshold, p.ego_nearest_yaw_threshold); const auto max_path_velocity = @@ -914,8 +914,9 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( } auto prepare_segment = prev_module_output_.path; - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prepare_segment.points, getEgoPose(), 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); return prepare_segment; @@ -1667,7 +1668,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); } - const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( + const auto lane_changing_start_pose = autoware_motion_utils::calcLongitudinalOffsetPose( prev_module_output_.path.points, current_lane_terminal_point, -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); @@ -1752,7 +1753,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( target_lane_reference_path, shift_length); auto reference_segment = prev_module_output_.path; - const double length_to_lane_changing_start = motion_utils::calcSignedArcLength( + const double length_to_lane_changing_start = autoware_motion_utils::calcSignedArcLength( reference_segment.points, reference_segment.points.front().point.pose.position, lane_changing_start_pose->position); utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); @@ -1894,12 +1895,12 @@ bool NormalLaneChange::calcAbortPath() lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, 0.0); const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( lane_changing_path.points, ref.points.back().point.pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); }); - const auto ego_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto ego_pose_idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( lane_changing_path.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); const auto get_abort_idx_and_distance = [&](const double param_time) { @@ -1981,10 +1982,10 @@ bool NormalLaneChange::calcAbortPath() // reference_lane_segment = terminal_path->path; // } const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose; - const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const auto seg_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold, common_param.ego_nearest_yaw_threshold); - reference_lane_segment.points = motion_utils::cropPoints( + reference_lane_segment.points = autoware_motion_utils::cropPoints( reference_lane_segment.points, return_pose.position, seg_idx, common_param.forward_path_length, 0.0); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 30d943deb791..7edd2d063348 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -25,14 +25,14 @@ #include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include +#include +#include #include #include #include #include #include -#include -#include -#include #include #include @@ -368,8 +368,8 @@ std::optional constructCandidatePath( LaneChangePath candidate_path; candidate_path.info = lane_change_info; - const auto lane_change_end_idx = - motion_utils::findNearestIndex(shifted_path.path.points, candidate_path.info.lane_changing_end); + const auto lane_change_end_idx = autoware_motion_utils::findNearestIndex( + shifted_path.path.points, candidate_path.info.lane_changing_end); if (!lane_change_end_idx) { RCLCPP_DEBUG(get_logger(), "Lane change end idx not found on target path."); @@ -385,7 +385,7 @@ std::optional constructCandidatePath( continue; } const auto nearest_idx = - motion_utils::findNearestIndex(target_segment.points, point.point.pose); + autoware_motion_utils::findNearestIndex(target_segment.points, point.point.pose); point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; } @@ -473,10 +473,10 @@ ShiftLine getLaneChangingShiftLine( shift_line.end_shift_length = shift_length; shift_line.start = lane_changing_start_pose; shift_line.end = lane_changing_end_pose; - shift_line.start_idx = - motion_utils::findNearestIndex(reference_path.points, lane_changing_start_pose.position); + shift_line.start_idx = autoware_motion_utils::findNearestIndex( + reference_path.points, lane_changing_start_pose.position); shift_line.end_idx = - motion_utils::findNearestIndex(reference_path.points, lane_changing_end_pose.position); + autoware_motion_utils::findNearestIndex(reference_path.points, lane_changing_end_pose.position); return shift_line; } @@ -768,9 +768,9 @@ CandidateOutput assignToCandidate( CandidateOutput candidate_output; candidate_output.path_candidate = lane_change_path.path; candidate_output.lateral_shift = utils::lane_change::getLateralShift(lane_change_path); - candidate_output.start_distance_to_path_change = motion_utils::calcSignedArcLength( + candidate_output.start_distance_to_path_change = autoware_motion_utils::calcSignedArcLength( lane_change_path.path.points, ego_position, lane_change_path.info.shift_line.start.position); - candidate_output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( + candidate_output.finish_distance_to_path_change = autoware_motion_utils::calcSignedArcLength( lane_change_path.path.points, ego_position, lane_change_path.info.shift_line.end.position); return candidate_output; @@ -806,9 +806,10 @@ std::vector convertToPredictedPath( const auto & minimum_lane_changing_velocity = lane_change_parameters.minimum_lane_changing_velocity; - const auto nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); + const auto nearest_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); std::vector predicted_path; const auto vehicle_pose_frenet = @@ -821,7 +822,7 @@ std::vector convertToPredictedPath( std::max(initial_velocity + prepare_acc * t, minimum_lane_changing_velocity); const double length = initial_velocity * t + 0.5 * prepare_acc * t * t; const auto pose = - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + autoware_motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } @@ -836,7 +837,7 @@ std::vector convertToPredictedPath( const double length = lane_changing_velocity * delta_t + 0.5 * lane_changing_acc * delta_t * delta_t + offset; const auto pose = - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + autoware_motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } @@ -869,7 +870,7 @@ bool isParkedObject( const auto & object_pose = object.initial_pose.pose; const auto object_closest_index = - motion_utils::findNearestIndex(path.points, object_pose.position); + autoware_motion_utils::findNearestIndex(path.points, object_pose.position); const auto object_closest_pose = path.points.at(object_closest_index).point.pose; lanelet::ConstLanelet closest_lanelet; @@ -877,7 +878,8 @@ bool isParkedObject( return false; } - const double lat_dist = motion_utils::calcLateralOffset(path.points, object_pose.position); + const double lat_dist = + autoware_motion_utils::calcLateralOffset(path.points, object_pose.position); lanelet::BasicLineString2d bound; double center_to_bound_buffer = 0.0; if (lat_dist > 0.0) { @@ -1004,12 +1006,12 @@ bool passParkedObject( for (const auto & point : leading_obj_poly.outer()) { const auto obj_p = autoware_universe_utils::createPoint(point.x(), point.y(), 0.0); const double dist = - motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, current_path_end); + autoware_motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, current_path_end); min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); if (is_goal_in_route) { const auto goal_pose = route_handler.getGoalPose(); - const double dist_to_goal = - motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, goal_pose.position); + const double dist_to_goal = autoware_motion_utils::calcSignedArcLength( + current_lane_path.points, obj_p, goal_pose.position); min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal); } } @@ -1059,15 +1061,15 @@ std::optional getLeadingStaticObjectIdx( continue; } - const double dist_back_to_obj = motion_utils::calcSignedArcLength( + const double dist_back_to_obj = autoware_motion_utils::calcSignedArcLength( path.points, path_end.point.pose.position, obj_pose.position); if (dist_back_to_obj > 0.0) { // object is not on the lane change path continue; } - const double dist_lc_start_to_obj = - motion_utils::calcSignedArcLength(path.points, lane_change_start.position, obj_pose.position); + const double dist_lc_start_to_obj = autoware_motion_utils::calcSignedArcLength( + path.points, lane_change_start.position, obj_pose.position); if (dist_lc_start_to_obj < 0.0) { // object is on the lane changing path or behind it. It will be detected in safety check continue; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 639d0c9ca084..a4eeba6051b6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -41,6 +41,7 @@ autoware_freespace_planning_algorithms autoware_frenet_planner autoware_lane_departure_checker + autoware_motion_utils autoware_path_sampler autoware_perception_msgs autoware_planning_msgs @@ -55,7 +56,6 @@ libboost-dev libopencv-dev magic_enum - motion_utils object_recognition_utils pluginlib rclcpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 6878fcc08397..378d8f9c5a5f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include #include @@ -445,7 +445,7 @@ void BehaviorPathPlannerNode::run() const auto current_pose = planner_data_->self_odometry->pose.pose; if (!path->points.empty()) { const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(path->points); - path->points = motion_utils::cropPoints( + path->points = autoware_motion_utils::cropPoints( path->points, current_pose.position, current_seg_idx, planner_data_->parameters.forward_path_length, planner_data_->parameters.backward_path_length + @@ -744,8 +744,8 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = - motion_utils::convertToPath(*path_candidate_ptr); + output = autoware_motion_utils::convertToPath( + *path_candidate_ptr); // header is replaced by the input one, so it is substituted again output.header = planner_data->route_handler->getRouteHeader(); output.header.stamp = this->now(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 61c7d45b22c7..71714cda9052 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -219,7 +219,7 @@ void PlannerManager::generateCombinedDrivableArea( const auto & di = output.drivable_area_info; constexpr double epsilon = 1e-3; - const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path.points); + const auto is_driving_forward_opt = autoware_motion_utils::isDrivingForward(output.path.points); const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (epsilon < std::abs(di.drivable_margin)) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp index fdbabe8c7107..f79087d7df40 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp @@ -13,12 +13,12 @@ // limitations under the License. #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include "input.hpp" #include "lanelet2_core/Attribute.h" #include "lanelet2_core/geometry/LineString.h" #include "lanelet2_core/geometry/Point.h" #include "lanelet2_core/primitives/Lanelet.h" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -34,8 +34,9 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnStraightLin autoware::behavior_path_planner::generateStraightSamplePathWithLaneId(0.0f, 1.0f, 10u); Pose vehicle_pose = autoware::behavior_path_planner::generateEgoSamplePose(10.7f, -1.7f, 0.0); - const size_t vehicle_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, vehicle_pose, 3.0, 1.0); + const size_t vehicle_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, vehicle_pose, 3.0, 1.0); const auto vehicle_pose_frenet = autoware::behavior_path_planner::utils::convertToFrenetPoint( path.points, vehicle_pose.position, vehicle_seg_idx); @@ -49,8 +50,9 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnDiagonalLin autoware::behavior_path_planner::generateDiagonalSamplePathWithLaneId(0.0f, 1.0f, 10u); Pose vehicle_pose = autoware::behavior_path_planner::generateEgoSamplePose(0.1f, 0.1f, 0.0); - const size_t vehicle_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, vehicle_pose, 3.0, 1.0); + const size_t vehicle_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, vehicle_pose, 3.0, 1.0); const auto vehicle_pose_frenet = autoware::behavior_path_planner::utils::convertToFrenetPoint( path.points, vehicle_pose.position, vehicle_seg_idx); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp index 0240f4af7e4e..ec9a67a40e05 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp @@ -18,7 +18,7 @@ #include "autoware/behavior_path_planner_common/parameters.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include #include @@ -254,7 +254,7 @@ struct PlannerData template size_t findEgoIndex(const std::vector & points) const { - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( points, self_odometry->pose.pose, parameters.ego_nearest_dist_threshold, parameters.ego_nearest_yaw_threshold); } @@ -262,7 +262,7 @@ struct PlannerData template size_t findEgoSegmentIndex(const std::vector & points) const { - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, self_odometry->pose.pose, parameters.ego_nearest_dist_threshold, parameters.ego_nearest_yaw_threshold); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 1e145b898354..898414c29eaa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -22,6 +22,9 @@ #include #include +#include +#include +#include #include #include #include @@ -29,9 +32,6 @@ #include #include #include -#include -#include -#include #include #include @@ -146,7 +146,7 @@ class SceneModuleInterface updateData(); const auto output = isWaitingApproval() ? planWaitingApproval() : plan(); try { - motion_utils::validateNonEmpty(output.path.points); + autoware_motion_utils::validateNonEmpty(output.path.points); } catch (const std::exception & ex) { throw std::invalid_argument("[" + name_ + "]" + ex.what()); } @@ -566,8 +566,8 @@ class SceneModuleInterface StopFactor stop_factor; stop_factor.stop_pose = stop_pose_.value(); - stop_factor.dist_to_stop_pose = - motion_utils::calcSignedArcLength(path.points, getEgoPosition(), stop_pose_.value().position); + stop_factor.dist_to_stop_pose = autoware_motion_utils::calcSignedArcLength( + path.points, getEgoPosition(), stop_pose_.value().position); stop_reason_.stop_factors.push_back(stop_factor); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 78befd1451a0..02e29e028f74 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -36,10 +36,10 @@ namespace autoware::behavior_path_planner { +using autoware_motion_utils::createDeadLineVirtualWallMarker; +using autoware_motion_utils::createSlowDownVirtualWallMarker; +using autoware_motion_utils::createStopVirtualWallMarker; using autoware_universe_utils::toHexString; -using motion_utils::createDeadLineVirtualWallMarker; -using motion_utils::createSlowDownVirtualWallMarker; -using motion_utils::createStopVirtualWallMarker; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; using SceneModuleObserver = std::weak_ptr; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index b8acf91724c6..9196123c4e73 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ #include "autoware/behavior_path_planner_common/data_manager.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include #include @@ -93,9 +93,11 @@ FrenetPoint convertToFrenetPoint( FrenetPoint frenet_point; const double longitudinal_length = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, search_point_geom); - frenet_point.length = motion_utils::calcSignedArcLength(points, 0, seg_idx) + longitudinal_length; - frenet_point.distance = motion_utils::calcLateralOffset(points, search_point_geom, seg_idx); + autoware_motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, search_point_geom); + frenet_point.length = + autoware_motion_utils::calcSignedArcLength(points, 0, seg_idx) + longitudinal_length; + frenet_point.distance = + autoware_motion_utils::calcLateralOffset(points, search_point_geom, seg_idx); return frenet_point; } @@ -342,12 +344,12 @@ size_t findNearestSegmentIndex( const double yaw_threshold) { const auto nearest_idx = - motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); + autoware_motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); if (nearest_idx) { return nearest_idx.value(); } - return motion_utils::findNearestSegmentIndex(points, pose.position); + return autoware_motion_utils::findNearestSegmentIndex(points, pose.position); } } // namespace autoware::behavior_path_planner::utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index 2a7b595db6b9..46ee55c07fe0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -45,6 +45,7 @@ autoware_adapi_v1_msgs autoware_freespace_planning_algorithms autoware_lane_departure_checker + autoware_motion_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_msgs @@ -56,7 +57,6 @@ interpolation lanelet2_extension magic_enum - motion_utils object_recognition_utils rclcpp tf2 diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index f379d92afecf..5a785a71b863 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -16,15 +16,15 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include +#include +#include #include #include #include #include #include -#include -#include -#include -#include #include #include @@ -33,15 +33,16 @@ namespace autoware::behavior_path_planner { -using motion_utils::calcSignedArcLength; +using autoware_motion_utils::calcSignedArcLength; double calc_distance( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const Pose & input_point, const double nearest_dist_threshold, const double nearest_yaw_threshold) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return motion_utils::calcSignedArcLength( + const size_t nearest_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return autoware_motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); } @@ -82,7 +83,7 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( } // Closest ego segment - const size_t ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const size_t ego_seg_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( extended_path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); // Get closest intersection turn signal if exists @@ -230,20 +231,20 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } const size_t front_nearest_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, lane_front_pose, nearest_dist_threshold, nearest_yaw_threshold); const size_t back_nearest_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, lane_back_pose, nearest_dist_threshold, nearest_yaw_threshold); // Distance from ego vehicle front pose to front point of the lane - const double dist_to_front_point = motion_utils::calcSignedArcLength( + const double dist_to_front_point = autoware_motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, lane_front_pose.position, front_nearest_seg_idx) - base_link2front_; // Distance from ego vehicle base link to the terminal point of the lane - const double dist_to_back_point = motion_utils::calcSignedArcLength( + const double dist_to_back_point = autoware_motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, lane_back_pose.position, back_nearest_seg_idx); @@ -278,9 +279,10 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( const auto & turn_signal_info = signal_queue.front(); const auto & required_end_point = turn_signal_info.required_end_point; - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, required_end_point, nearest_dist_threshold, nearest_yaw_threshold); - const double dist_to_end_point = motion_utils::calcSignedArcLength( + const size_t nearest_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, required_end_point, nearest_dist_threshold, nearest_yaw_threshold); + const double dist_to_end_point = autoware_motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, required_end_point.position, nearest_seg_idx); @@ -404,9 +406,10 @@ TurnSignalInfo TurnSignalDecider::overwrite_turn_signal( } const auto get_distance = [&](const Pose & input_point) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return motion_utils::calcSignedArcLength( + const size_t nearest_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return autoware_motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx) - base_link2front_; @@ -430,9 +433,10 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const double nearest_dist_threshold, const double nearest_yaw_threshold) { const auto get_distance = [&](const Pose & input_point) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return motion_utils::calcSignedArcLength( + const size_t nearest_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return autoware_motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); }; @@ -553,9 +557,9 @@ geometry_msgs::msg::Pose TurnSignalDecider::get_required_end_point( for (size_t i = 0; i < centerline.size(); ++i) { converted_centerline.at(i).position = lanelet::utils::conversion::toGeomMsgPt(centerline[i]); } - motion_utils::insertOrientation(converted_centerline, true); + autoware_motion_utils::insertOrientation(converted_centerline, true); - const double length = motion_utils::calcArcLength(converted_centerline); + const double length = autoware_motion_utils::calcArcLength(converted_centerline); // Create resampling intervals const double resampling_interval = 1.0; @@ -565,14 +569,14 @@ geometry_msgs::msg::Pose TurnSignalDecider::get_required_end_point( } // Insert terminal point - if (length - resampling_arclength.back() < motion_utils::overlap_threshold) { + if (length - resampling_arclength.back() < autoware_motion_utils::overlap_threshold) { resampling_arclength.back() = length; } else { resampling_arclength.push_back(length); } const auto resampled_centerline = - motion_utils::resamplePoseVector(converted_centerline, resampling_arclength); + autoware_motion_utils::resamplePoseVector(converted_centerline, resampling_arclength); const double terminal_yaw = tf2::getYaw(resampled_centerline.back().orientation); for (size_t i = 0; i < resampled_centerline.size(); ++i) { @@ -592,9 +596,10 @@ void TurnSignalDecider::set_intersection_info( const double nearest_yaw_threshold) { const auto get_distance = [&](const Pose & input_point) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return motion_utils::calcSignedArcLength( + const size_t nearest_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return autoware_motion_utils::calcSignedArcLength( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 32cced71eec7..e3ebc07a8ce3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -20,11 +20,11 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include +#include +#include #include #include -#include -#include -#include #include @@ -48,8 +48,9 @@ void reuse_previous_poses( { std::vector cropped_poses; std::vector cropped_curvatures; - const auto ego_is_behind = prev_poses.size() > 1 && motion_utils::calcLongitudinalOffsetToSegment( - prev_poses, 0, ego_point) < 0.0; + const auto ego_is_behind = + prev_poses.size() > 1 && + autoware_motion_utils::calcLongitudinalOffsetToSegment(prev_poses, 0, ego_point) < 0.0; const auto ego_is_far = !prev_poses.empty() && autoware_universe_utils::calcDistance2d( ego_point, prev_poses.front()) < 0.0; // make sure the reused points are not behind the current original drivable area @@ -64,9 +65,9 @@ void reuse_previous_poses( if (!ego_is_behind && !ego_is_far && prev_poses.size() > 1 && !prev_poses_across_bounds) { const auto first_idx = - motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); + autoware_motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); const auto deviation = - motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); + autoware_motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); if (first_idx && deviation < params.max_reuse_deviation) { LineString2d path_ls; for (const auto & p : path.points) path_ls.push_back(convert_point(p.point.pose.position)); @@ -86,27 +87,27 @@ void reuse_previous_poses( } if (cropped_poses.empty()) { const auto resampled_path_points = - motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; + autoware_motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; const auto cropped_path = params.max_path_arc_length <= 0.0 ? resampled_path_points - : motion_utils::cropForwardPoints( + : autoware_motion_utils::cropForwardPoints( resampled_path_points, resampled_path_points.front().point.pose.position, 0, params.max_path_arc_length); for (const auto & p : cropped_path) cropped_poses.push_back(p.point.pose); } else { - const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses); - const auto max_path_arc_length = motion_utils::calcArcLength(path.points); - const auto first_arc_length = motion_utils::calcSignedArcLength( + const auto initial_arc_length = autoware_motion_utils::calcArcLength(cropped_poses); + const auto max_path_arc_length = autoware_motion_utils::calcArcLength(path.points); + const auto first_arc_length = autoware_motion_utils::calcSignedArcLength( path.points, path.points.front().point.pose.position, cropped_poses.back().position); for (auto arc_length = first_arc_length + params.resample_interval; (params.max_path_arc_length <= 0.0 || initial_arc_length + (arc_length - first_arc_length) <= params.max_path_arc_length) && arc_length <= max_path_arc_length; arc_length += params.resample_interval) - cropped_poses.push_back(motion_utils::calcInterpolatedPose(path.points, arc_length)); + cropped_poses.push_back(autoware_motion_utils::calcInterpolatedPose(path.points, arc_length)); } - prev_poses = motion_utils::removeOverlapPoints(cropped_poses); + prev_poses = autoware_motion_utils::removeOverlapPoints(cropped_poses); prev_curvatures = cropped_curvatures; } @@ -302,7 +303,7 @@ void expand_bound( std::vector calculate_smoothed_curvatures( const std::vector & poses, const size_t smoothing_window_size) { - const auto curvatures = motion_utils::calcCurvature(poses); + const auto curvatures = autoware_motion_utils::calcCurvature(poses); std::vector smoothed_curvatures(curvatures.size()); for (auto i = 0UL; i < curvatures.size(); ++i) { auto sum = 0.0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index be7a5cac7f0a..87e7d297c65c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -14,14 +14,14 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include #include #include #include #include #include -#include #include @@ -90,7 +90,7 @@ size_t findNearestSegmentIndexFromLateralDistance( double min_lateral_dist = std::numeric_limits::max(); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); + autoware_motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); const double lat_dist = [&]() { if (lon_dist < 0.0) { @@ -99,7 +99,7 @@ size_t findNearestSegmentIndexFromLateralDistance( if (segment_length < lon_dist) { return calcDistance2d(points.at(seg_idx + 1), target_point); } - return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + return std::abs(autoware_motion_utils::calcLateralOffset(points, target_point, seg_idx)); }(); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; @@ -111,7 +111,7 @@ size_t findNearestSegmentIndexFromLateralDistance( return *closest_idx; } - return motion_utils::findNearestSegmentIndex(points, target_point); + return autoware_motion_utils::findNearestSegmentIndex(points, target_point); } template @@ -132,8 +132,8 @@ size_t findNearestSegmentIndexFromLateralDistance( if (yaw_threshold < std::abs(yaw)) { continue; } - const double lon_dist = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point.position); + const double lon_dist = autoware_motion_utils::calcLongitudinalOffsetToSegment( + points, seg_idx, target_point.position); const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); const double lat_dist = [&]() { if (lon_dist < 0.0) { @@ -142,7 +142,8 @@ size_t findNearestSegmentIndexFromLateralDistance( if (segment_length < lon_dist) { return calcDistance2d(points.at(seg_idx + 1), target_point.position); } - return std::abs(motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); + return std::abs( + autoware_motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); }(); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; @@ -154,7 +155,7 @@ size_t findNearestSegmentIndexFromLateralDistance( return *closest_idx; } - return motion_utils::findNearestSegmentIndex(points, target_point.position); + return autoware_motion_utils::findNearestSegmentIndex(points, target_point.position); } bool checkHasSameLane( @@ -176,10 +177,10 @@ geometry_msgs::msg::Point calcLongitudinalOffsetStartPoint( const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) { - const double offset_length = - motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); - const auto offset_point = - motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); + const double offset_length = autoware_motion_utils::calcLongitudinalOffsetToSegment( + points, nearest_segment_idx, pose.position); + const auto offset_point = autoware_motion_utils::calcLongitudinalOffsetPoint( + points, nearest_segment_idx, offset_length + offset); return offset_point ? offset_point.value() : points.at(nearest_segment_idx); } @@ -188,10 +189,10 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) { - const double offset_length = - motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); - const auto offset_point = - motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); + const double offset_length = autoware_motion_utils::calcLongitudinalOffsetToSegment( + points, nearest_segment_idx, pose.position); + const auto offset_point = autoware_motion_utils::calcLongitudinalOffsetPoint( + points, nearest_segment_idx, offset_length + offset); return offset_point ? offset_point.value() : points.at(nearest_segment_idx + 1); } @@ -293,10 +294,10 @@ PolygonPoint transformBoundFrenetCoordinate( const size_t min_dist_seg_idx = std::distance( dist_to_bound_segment_vec.begin(), std::min_element(dist_to_bound_segment_vec.begin(), dist_to_bound_segment_vec.end())); - const double lon_dist_to_segment = - motion_utils::calcLongitudinalOffsetToSegment(bound_points, min_dist_seg_idx, target_point); + const double lon_dist_to_segment = autoware_motion_utils::calcLongitudinalOffsetToSegment( + bound_points, min_dist_seg_idx, target_point); const double lat_dist_to_segment = - motion_utils::calcLateralOffset(bound_points, target_point, min_dist_seg_idx); + autoware_motion_utils::calcLateralOffset(bound_points, target_point, min_dist_seg_idx); return PolygonPoint{target_point, min_dist_seg_idx, lon_dist_to_segment, lat_dist_to_segment}; } @@ -342,7 +343,7 @@ std::vector generatePolygonInsideBounds( if (!intersection) { continue; } - const double lon_dist = motion_utils::calcLongitudinalOffsetToSegment( + const double lon_dist = autoware_motion_utils::calcLongitudinalOffsetToSegment( bound, intersection->first, intersection->second); const auto intersect_point = PolygonPoint{intersection->second, intersection->first, lon_dist, 0.0}; @@ -568,7 +569,7 @@ std::vector getPolygonPointsInsideBounds( // add start and end points projected to bound if necessary if (inside_polygon.at(start_idx).lat_dist_to_bound != 0.0) { // not on bound auto start_point = inside_polygon.at(start_idx); - const auto start_point_on_bound = motion_utils::calcLongitudinalOffsetPoint( + const auto start_point_on_bound = autoware_motion_utils::calcLongitudinalOffsetPoint( bound, start_point.bound_seg_idx, start_point.lon_dist_to_segment); if (start_point_on_bound) { start_point.point = start_point_on_bound.value(); @@ -577,7 +578,7 @@ std::vector getPolygonPointsInsideBounds( } if (inside_polygon.at(end_idx).lat_dist_to_bound != 0.0) { // not on bound auto end_point = inside_polygon.at(end_idx); - const auto end_point_on_bound = motion_utils::calcLongitudinalOffsetPoint( + const auto end_point_on_bound = autoware_motion_utils::calcLongitudinalOffsetPoint( bound, end_point.bound_seg_idx, end_point.lon_dist_to_segment); if (end_point_on_bound) { end_point.point = end_point_on_bound.value(); @@ -605,7 +606,7 @@ std::vector updateBoundary( const auto & start_poly = polygon.front(); const auto & end_poly = polygon.back(); - const double front_offset = motion_utils::calcLongitudinalOffsetToSegment( + const double front_offset = autoware_motion_utils::calcLongitudinalOffsetToSegment( updated_bound, start_poly.bound_seg_idx, start_poly.point); const size_t removed_start_idx = @@ -864,7 +865,7 @@ void generateDrivableArea( const auto & prev_point = resampled_path.points.back().point.pose.position; const auto & curr_point = path.points.at(i).point.pose.position; const double signed_arc_length = - motion_utils::calcSignedArcLength(path.points, prev_point, curr_point); + autoware_motion_utils::calcSignedArcLength(path.points, prev_point, curr_point); if (signed_arc_length > resample_interval) { resampled_path.points.push_back(path.points.at(i)); } @@ -1041,7 +1042,7 @@ void extractObstaclesFromDrivableArea( // get edge points of the object const size_t nearest_path_idx = - motion_utils::findNearestIndex(path.points, obj_pos); // to get z for object polygon + autoware_motion_utils::findNearestIndex(path.points, obj_pos); // to get z for object polygon std::vector edge_points; for (int i = 0; i < static_cast(obstacle.poly.outer().size()) - 1; ++i) { // NOTE: There is a duplicated points @@ -1557,7 +1558,7 @@ std::vector postProcess( const auto start_idx = [&]() { const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); - const auto cropped_path_points = motion_utils::cropPoints( + const auto cropped_path_points = autoware_motion_utils::cropPoints( path.points, current_pose.position, current_seg_idx, planner_data->parameters.forward_path_length, planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); @@ -1619,7 +1620,7 @@ std::vector calcBound( const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward) { - using motion_utils::removeOverlapPoints; + using autoware_motion_utils::removeOverlapPoints; const auto & route_handler = planner_data->route_handler; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index b7c468ebd3d8..92d955edb454 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -70,7 +70,7 @@ PathWithLaneId GeometricParallelParking::getFullPath() const } PathWithLaneId filtered_path = path; - filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + filtered_path.points = autoware_motion_utils::removeOverlapPoints(filtered_path.points); return filtered_path; } @@ -299,7 +299,7 @@ bool GeometricParallelParking::planPullOut( paths.back().points.end(), road_center_line_path.points.begin() + 1, // to avoid overlapped point road_center_line_path.points.end()); - paths.back().points = motion_utils::removeOverlapPoints(paths.back().points); + paths.back().points = autoware_motion_utils::removeOverlapPoints(paths.back().points); // if the end point is the goal, set the velocity to 0 if (path_terminal_is_goal) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp index 622f4ac75c3d..3a9f70c16fb3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -16,13 +16,13 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include -#include namespace autoware::behavior_path_planner::utils::parking_departure { -using motion_utils::calcDecelDistWithJerkAndAccConstraints; +using autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints; std::optional calcFeasibleDecelDistance( std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, @@ -58,7 +58,7 @@ void modifyVelocityByDirection( auto pair_itr = std::begin(terminal_vel_acc_pairs); for (; path_itr != std::end(paths); ++path_itr, ++pair_itr) { - const auto is_driving_forward = motion_utils::isDrivingForward(path_itr->points); + const auto is_driving_forward = autoware_motion_utils::isDrivingForward(path_itr->points); // If the number of points in the path is less than 2, don't insert stop velocity and // set pairs_terminal_velocity_and_accel to 0 @@ -144,7 +144,7 @@ std::optional generateFeasibleStopPath( } // set stop point - const auto stop_idx = motion_utils::insertStopPoint( + const auto stop_idx = autoware_motion_utils::insertStopPoint( planner_data->self_odometry->pose.pose, *min_stop_distance, current_path.points); if (!stop_idx) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 0514680f49d9..ca72be6fa156 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -17,8 +17,8 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include #include -#include #include @@ -39,7 +39,7 @@ bool position_filter( const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance) { - const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( + const auto dist_ego_to_obj = autoware_motion_utils::calcSignedArcLength( path_points, current_pose, object.kinematics.initial_pose_with_covariance.pose.position); return (backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance); @@ -274,7 +274,7 @@ std::vector createPredictedPath( } const auto pose = - motion_utils::calcInterpolatedPose(path_points, vehicle_pose_frenet.length + length); + autoware_motion_utils::calcInterpolatedPose(path_points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 4b3910e351f8..57ce48c54004 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -15,10 +15,10 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/ros/uuid_helper.hpp" #include "interpolation/linear_interpolation.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -81,7 +81,7 @@ bool isTargetObjectFront( const auto obj_polygon_outer = obj_polygon.outer(); for (const auto & obj_edge : obj_polygon_outer) { const auto obj_point = autoware_universe_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); - if (motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { + if (autoware_motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { return true; } } @@ -208,7 +208,7 @@ Polygon2d createExtendedPolygonAlongPath( debug.lat_offset = lat_offset; } - const auto lon_offset_pose = motion_utils::calcLongitudinalOffsetPose( + const auto lon_offset_pose = autoware_motion_utils::calcLongitudinalOffsetPose( planned_path.points, base_link_pose.position, lon_length); if (!lon_offset_pose.has_value()) { return createExtendedPolygon( @@ -216,9 +216,9 @@ Polygon2d createExtendedPolygonAlongPath( } const size_t start_idx = - motion_utils::findNearestSegmentIndex(planned_path.points, base_link_pose.position); - const size_t end_idx = - motion_utils::findNearestSegmentIndex(planned_path.points, lon_offset_pose.value().position); + autoware_motion_utils::findNearestSegmentIndex(planned_path.points, base_link_pose.position); + const size_t end_idx = autoware_motion_utils::findNearestSegmentIndex( + planned_path.points, lon_offset_pose.value().position); Polygon2d polygon; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index ba7d6d70dae0..fa2c45b74942 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -16,9 +16,9 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include #include #include -#include #include #include @@ -51,10 +51,10 @@ std::string toStr(const std::vector & v) namespace autoware::behavior_path_planner { -using motion_utils::findNearestIndex; -using motion_utils::insertOrientation; -using motion_utils::removeFirstInvalidOrientationPoints; -using motion_utils::removeOverlapPoints; +using autoware_motion_utils::findNearestIndex; +using autoware_motion_utils::insertOrientation; +using autoware_motion_utils::removeFirstInvalidOrientationPoints; +using autoware_motion_utils::removeOverlapPoints; void PathShifter::setPath(const PathWithLaneId & path) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 2e79e54499c3..1e2ceac5cd19 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -17,12 +17,12 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include #include #include #include #include -#include -#include #include @@ -98,8 +98,8 @@ PathWithLaneId resamplePathWithSpline( // Get lane ids that are not duplicated std::vector s_in; std::unordered_set unique_lane_ids; - const auto s_vec = - motion_utils::calcSignedArcLengthPartialSum(transformed_path, 0, transformed_path.size()); + const auto s_vec = autoware_motion_utils::calcSignedArcLengthPartialSum( + transformed_path, 0, transformed_path.size()); for (size_t i = 0; i < path.points.size(); ++i) { const double s = s_vec.at(i); for (const auto & lane_id : path.points.at(i).lane_ids) { @@ -129,7 +129,8 @@ PathWithLaneId resamplePathWithSpline( } // Insert Stop Point - const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint(transformed_path); + const auto closest_stop_dist = + autoware_motion_utils::calcDistanceToForwardStopPoint(transformed_path); if (closest_stop_dist) { const auto close_indices = find_almost_same_values(s_out, *closest_stop_dist); if (close_indices) { @@ -152,7 +153,7 @@ PathWithLaneId resamplePathWithSpline( std::sort(s_out.begin(), s_out.end()); - return motion_utils::resamplePath(path, s_out); + return autoware_motion_utils::resamplePath(path, s_out); } size_t getIdxByArclength( @@ -184,7 +185,7 @@ size_t getIdxByArclength( return 0; } -// TODO(murooka) This function should be replaced with motion_utils::cropPoints +// TODO(murooka) This function should be replaced with autoware_motion_utils::cropPoints void clipPathLength( PathWithLaneId & path, const size_t target_idx, const double forward, const double backward) { @@ -201,7 +202,7 @@ void clipPathLength( path.points = clipped_points; } -// TODO(murooka) This function should be replaced with motion_utils::cropPoints +// TODO(murooka) This function should be replaced with autoware_motion_utils::cropPoints void clipPathLength( PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params) { @@ -411,7 +412,7 @@ std::vector dividePath( void correctDividedPathVelocity(std::vector & divided_paths) { for (auto & path : divided_paths) { - const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + const auto is_driving_forward = autoware_motion_utils::isDrivingForward(path.points); // If the number of points in the path is less than 2, don't correct the velocity if (!is_driving_forward) { continue; @@ -507,11 +508,13 @@ Pose getUnshiftedEgoPose(const Pose & ego_pose, const ShiftedPath & prev_path) } // un-shifted for current ideal pose - const auto closest_idx = motion_utils::findNearestIndex(prev_path.path.points, ego_pose.position); + const auto closest_idx = + autoware_motion_utils::findNearestIndex(prev_path.path.points, ego_pose.position); // NOTE: Considering avoidance by motion, we set unshifted_pose as previous path instead of // ego_pose. - auto unshifted_pose = motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; + auto unshifted_pose = + autoware_motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; unshifted_pose = autoware_universe_utils::calcOffsetPose( unshifted_pose, 0.0, -prev_path.shift_length.at(closest_idx), 0.0); @@ -571,7 +574,7 @@ PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); PathWithLaneId filtered_path = path; - filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + filtered_path.points = autoware_motion_utils::removeOverlapPoints(filtered_path.points); return filtered_path; } @@ -612,10 +615,11 @@ BehaviorModuleOutput getReferencePath( // clip backward length // NOTE: In order to keep backward_path_length at least, resampling interval is added to the // backward. - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - reference_path.points, no_shift_pose, p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold); - reference_path.points = motion_utils::cropPoints( + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + reference_path.points, no_shift_pose, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold); + reference_path.points = autoware_motion_utils::cropPoints( reference_path.points, no_shift_pose.position, current_seg_idx, p.forward_path_length, p.backward_path_length + p.input_path_interval); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp index d92cdd192de3..db41e59e63db 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -13,12 +13,12 @@ // limitations under the License. #include -#include +#include #include namespace autoware::behavior_path_planner::utils::traffic_light { -using motion_utils::calcSignedArcLength; +using autoware_motion_utils::calcSignedArcLength; double getDistanceToNextTrafficLight( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index e81a928d1281..c6a6256ef43c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -14,16 +14,16 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include #include #include #include #include #include #include -#include #include @@ -43,10 +43,11 @@ double calcInterpolatedZ( const tier4_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Point target_pos, const size_t seg_idx) { - const double closest_to_target_dist = motion_utils::calcSignedArcLength( + const double closest_to_target_dist = autoware_motion_utils::calcSignedArcLength( input.points, input.points.at(seg_idx).point.pose.position, target_pos); // TODO(murooka) implement calcSignedArcLength(points, idx, point) - const double seg_dist = motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + const double seg_dist = + autoware_motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); const double closest_z = input.points.at(seg_idx).point.pose.position.z; const double next_z = input.points.at(seg_idx + 1).point.pose.position.z; @@ -60,7 +61,8 @@ double calcInterpolatedZ( double calcInterpolatedVelocity( const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) { - const double seg_dist = motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + const double seg_dist = + autoware_motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); const double closest_vel = input.points.at(seg_idx).point.longitudinal_velocity_mps; const double next_vel = input.points.at(seg_idx + 1).point.longitudinal_velocity_mps; @@ -439,7 +441,7 @@ PathWithLaneId refinePathForGoal( { PathWithLaneId filtered_path = input; PathWithLaneId path_with_goal; - filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + filtered_path.points = autoware_motion_utils::removeOverlapPoints(filtered_path.points); // always set zero velocity at the end of path for safety if (!filtered_path.points.empty()) { @@ -780,7 +782,7 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path) const size_t original_size = path.points.size(); // insert stop point - const auto insert_idx = motion_utils::insertStopPoint(length, path.points); + const auto insert_idx = autoware_motion_utils::insertStopPoint(length, path.points); if (!insert_idx) { return PathPointWithLaneId(); } @@ -1148,13 +1150,14 @@ PathWithLaneId getCenterLinePath( const auto raw_path_with_lane_id = route_handler.getCenterLinePath(lanelet_sequence, s_backward, s_forward, true); - auto resampled_path_with_lane_id = motion_utils::resamplePath( + auto resampled_path_with_lane_id = autoware_motion_utils::resamplePath( raw_path_with_lane_id, parameter.input_path_interval, parameter.enable_akima_spline_first); // convert centerline, which we consider as CoG center, to rear wheel center if (parameter.enable_cog_on_centerline) { const double rear_to_cog = parameter.vehicle_length / 2 - parameter.rear_overhang; - return motion_utils::convertToRearWheelCenter(resampled_path_with_lane_id, rear_to_cog); + return autoware_motion_utils::convertToRearWheelCenter( + resampled_path_with_lane_id, rear_to_cog); } return resampled_path_with_lane_id; @@ -1193,7 +1196,7 @@ PathWithLaneId setDecelerationVelocity( for (auto & point : reference_path.points) { const auto arclength_to_target = std::max( - 0.0, motion_utils::calcSignedArcLength( + 0.0, autoware_motion_utils::calcSignedArcLength( reference_path.points, point.point.pose.position, target_pose.position) + buffer); if (arclength_to_target > deceleration_interval) continue; @@ -1206,7 +1209,8 @@ PathWithLaneId setDecelerationVelocity( } const auto stop_point_length = - motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + buffer; + autoware_motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + + buffer; constexpr double eps{0.01}; if (std::abs(target_velocity) < eps && stop_point_length > 0.0) { const auto stop_point = utils::insertStopPoint(stop_point_length, reference_path); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp index 51e3a216c392..342ebc985a51 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_turn_signal.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include @@ -105,8 +105,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose on the behavior desired start { Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -116,8 +117,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is right before the intersection required start { Pose current_pose = generateEgoSamplePose(34.99f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -127,8 +129,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is within the intersection required section { Pose current_pose = generateEgoSamplePose(40.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -138,8 +141,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is within the intersection and behavior required section { Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -149,8 +153,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is on the intersection required end { Pose current_pose = generateEgoSamplePose(48.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -160,8 +165,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is right after the intersection required end { Pose current_pose = generateEgoSamplePose(48.1f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -171,8 +177,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is on the behavior required end { Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -182,8 +189,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is right after the behavior required end { Pose current_pose = generateEgoSamplePose(50.1f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -193,8 +201,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is right on the intersection desired end { Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -204,8 +213,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition1) // current pose is right right after the intersection desired end { Pose current_pose = generateEgoSamplePose(65.1f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -244,8 +254,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose on the behavior desired start { Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -255,8 +266,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is right before the intersection required start { Pose current_pose = generateEgoSamplePose(34.99f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -266,8 +278,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is on the behavior required start { Pose current_pose = generateEgoSamplePose(40.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -277,8 +290,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is on the behavior required end { Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -288,8 +302,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is on the intersection required end { Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -299,8 +314,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is right after the intersection required end { Pose current_pose = generateEgoSamplePose(50.1f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -310,8 +326,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition2) // current pose is on the intersection desired end { Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -351,8 +368,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose on the behavior desired start { Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -362,8 +380,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right before the behavior required start { Pose current_pose = generateEgoSamplePose(29.9f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -373,8 +392,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right on the behavior required start { Pose current_pose = generateEgoSamplePose(30.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -384,8 +404,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right before the intersection required start { Pose current_pose = generateEgoSamplePose(33.9f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -395,8 +416,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right on the intersection required start { Pose current_pose = generateEgoSamplePose(35.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -406,8 +428,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right before the behavior required end { Pose current_pose = generateEgoSamplePose(44.9f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -417,8 +440,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right on the behavior required end { Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -428,8 +452,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right before the intersection required end { Pose current_pose = generateEgoSamplePose(49.9f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -439,8 +464,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right on the intersection required end { Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); @@ -450,8 +476,9 @@ TEST(BehaviorPathPlanningTurnSignal, Condition3) // current pose is right on the intersection desired end { Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, 3.0, 1.0); + const size_t current_seg_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); const auto result_signal = turn_signal_decider.resolve_turn_signal( path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info, nearest_dist_threshold, nearest_yaw_threshold); diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index fd71b7d171a4..9ef227783bd9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" #include "autoware/behavior_path_sampling_planner_module/util.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/constants.hpp" @@ -38,7 +39,6 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_planning_msgs/msg/lateral_offset.hpp" @@ -204,7 +204,7 @@ class SamplingPlannerModule : public SceneModuleInterface if (length_to_goal < epsilon) return isReferencePathSafe(); const auto nearest_index = - motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose); + autoware_motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose); if (!nearest_index) return false; auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double { const auto rpy = autoware_universe_utils::getRPY(quat); diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml index 3b3c699f8f85..aab393f4bdb5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml @@ -16,6 +16,7 @@ autoware_behavior_path_planner_common autoware_bezier_sampler autoware_frenet_planner + autoware_motion_utils autoware_path_sampler autoware_perception_msgs autoware_planning_msgs @@ -27,7 +28,6 @@ geometry_msgs interpolation lanelet2_extension - motion_utils pluginlib rclcpp rclcpp_components diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 87fd2156b0eb..0e8e56489d6d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -16,14 +16,14 @@ namespace autoware::behavior_path_planner { +using autoware_motion_utils::calcSignedArcLength; +using autoware_motion_utils::findNearestIndex; +using autoware_motion_utils::findNearestSegmentIndex; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::calcOffsetPose; using autoware_universe_utils::getPoint; using autoware_universe_utils::Point2d; using geometry_msgs::msg::Point; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; namespace bg = boost::geometry; namespace bgi = boost::geometry::index; @@ -190,7 +190,7 @@ bool SamplingPlannerModule::isExecutionRequested() const return false; } - if (!motion_utils::isDrivingForward(getPreviousModuleOutput().reference_path.points)) { + if (!autoware_motion_utils::isDrivingForward(getPreviousModuleOutput().reference_path.points)) { RCLCPP_WARN(getLogger(), "Backward path is NOT supported. Just converting path to trajectory"); return false; } @@ -343,7 +343,7 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( point.lane_ids = path.points.at(i - 1).lane_ids; } if (reference_path_ptr) { - const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( reference_path_ptr->points, point.point.pose); const auto & closest_point = reference_path_ptr->points[idx]; point.point.longitudinal_velocity_mps = closest_point.point.longitudinal_velocity_mps; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml index e998bd7db83e..96b305a63a45 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml @@ -20,10 +20,10 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_planning_msgs autoware_universe_utils geometry_msgs - motion_utils pluginlib rclcpp tier4_planning_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index 7cb3ab04548e..065d15fbc893 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -28,12 +28,12 @@ namespace autoware::behavior_path_planner { +using autoware_motion_utils::calcSignedArcLength; +using autoware_motion_utils::findNearestIndex; +using autoware_motion_utils::findNearestSegmentIndex; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::getPoint; using geometry_msgs::msg::Point; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; SideShiftModule::SideShiftModule( const std::string & name, rclcpp::Node & node, @@ -373,7 +373,7 @@ double SideShiftModule::getClosestShiftLength() const } const auto ego_point = planner_data_->self_odometry->pose.pose.position; - const auto closest = motion_utils::findNearestIndex(prev_output_.path.points, ego_point); + const auto closest = autoware_motion_utils::findNearestIndex(prev_output_.path.points, ego_point); return prev_output_.shift_length.at(closest); } @@ -396,7 +396,7 @@ BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & pat auto output_path = path.path; const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(output_path.points); const auto & current_pose = planner_data_->self_odometry->pose.pose; - output_path.points = motion_utils::cropPoints( + output_path.points = autoware_motion_utils::cropPoints( output_path.points, current_pose.position, current_seg_idx, p.forward_path_length, p.backward_path_length + p.input_path_interval); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml index eda8328b07f6..77f83993f2cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml @@ -22,9 +22,9 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_rtc_interface autoware_universe_utils - motion_utils pluginlib rclcpp tier4_planning_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 060286517a1b..6e4928e0fbb7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -22,10 +22,10 @@ #include +using autoware_motion_utils::findNearestIndex; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::calcOffsetPose; using lanelet::utils::getArcCoordinates; -using motion_utils::findNearestIndex; namespace autoware::behavior_path_planner { using start_planner_utils::getPullOutLanes; @@ -92,14 +92,14 @@ std::optional GeometricPullOut::plan( // insert stop velocity to first arc path end output.partial_paths.front().points.back().point.longitudinal_velocity_mps = 0.0; const double arc_length_on_first_arc_path = - motion_utils::calcArcLength(output.partial_paths.front().points); + autoware_motion_utils::calcArcLength(output.partial_paths.front().points); const double time_to_center = arc_length_on_first_arc_path / (2 * velocity); const double average_velocity = arc_length_on_first_arc_path / (time_to_center * 2); const double average_acceleration = average_velocity / (time_to_center * 2); output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(average_velocity, average_acceleration)); const double arc_length_on_second_arc_path = - motion_utils::calcArcLength(planner_.getArcPaths().at(1).points); + autoware_motion_utils::calcArcLength(planner_.getArcPaths().at(1).points); output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { @@ -109,7 +109,7 @@ std::optional GeometricPullOut::plan( // Calculate the acceleration required to reach the forward parking velocity at the center of // the path, assuming constant acceleration and deceleration. - const double arc_length_on_path = motion_utils::calcArcLength(combined_path.points); + const double arc_length_on_path = autoware_motion_utils::calcArcLength(combined_path.points); output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 23fa9535b510..c004596de6ff 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -19,18 +19,18 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_start_planner_module/util.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include #include +using autoware_motion_utils::findNearestIndex; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::calcOffsetPose; using lanelet::utils::getArcCoordinates; -using motion_utils::findNearestIndex; namespace autoware::behavior_path_planner { using start_planner_utils::getPullOutLanes; @@ -107,9 +107,10 @@ std::optional ShiftPullOut::plan( // this ensures that the backward_path stays within the drivable area when starting from a // narrow place. - const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); + const size_t start_segment_idx = + autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( lanelet_map_ptr, shift_path, start_segment_idx); @@ -124,14 +125,16 @@ std::optional ShiftPullOut::plan( if (cropped_path.points.size() < 2) return false; const double max_long_offset = parameters_.maximum_longitudinal_deviation; const size_t start_segment_idx_after_crop = - motion_utils::findFirstNearestIndexWithSoftConstraints(cropped_path.points, start_pose); + autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + cropped_path.points, start_pose); // if the start segment id after crop is not 0, then the cropping is not excessive if (start_segment_idx_after_crop != 0) return true; - const auto long_offset_to_closest_point = motion_utils::calcLongitudinalOffsetToSegment( - cropped_path.points, start_segment_idx_after_crop, start_pose.position); - const auto long_offset_to_next_point = motion_utils::calcLongitudinalOffsetToSegment( + const auto long_offset_to_closest_point = + autoware_motion_utils::calcLongitudinalOffsetToSegment( + cropped_path.points, start_segment_idx_after_crop, start_pose.position); + const auto long_offset_to_next_point = autoware_motion_utils::calcLongitudinalOffsetToSegment( cropped_path.points, start_segment_idx_after_crop + 1, start_pose.position); return std::abs(long_offset_to_closest_point - long_offset_to_next_point) < max_long_offset; }; @@ -171,7 +174,7 @@ bool ShiftPullOut::refineShiftedPathToStartPose( size_t iteration = 0; while (iteration < MAX_ITERATION) { const double lateral_offset = - motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position); + autoware_motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position); PathShifter path_shifter; path_shifter.setPath(shifted_path.path); @@ -193,7 +196,7 @@ bool ShiftPullOut::refineShiftedPathToStartPose( if (is_within_tolerance( lateral_offset, - motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position), + autoware_motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position), TOLERANCE)) { return true; } @@ -419,7 +422,8 @@ double ShiftPullOut::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; - for (const auto & [k, segment_length] : motion_utils::calcCurvatureAndArcLength(path.points)) { + for (const auto & [k, segment_length] : + autoware_motion_utils::calcCurvatureAndArcLength(path.points)) { // after shifted segment length const double after_segment_length = k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index b53dc6861ea3..7c5030a9c69d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -20,7 +20,7 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_start_planner_module/debug.hpp" #include "autoware/behavior_path_start_planner_module/util.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include #include @@ -41,9 +41,9 @@ using autoware::behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using autoware_motion_utils::calcLateralOffset; +using autoware_motion_utils::calcLongitudinalOffsetPose; using autoware_universe_utils::calcOffsetPose; -using motion_utils::calcLateralOffset; -using motion_utils::calcLongitudinalOffsetPose; // set as macro so that calling function name will be printed. // debug print is heavy. turn on only when debugging. @@ -393,12 +393,12 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const const auto centerline_path = route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); const auto start_pose_nearest_segment_index = - motion_utils::findNearestSegmentIndex(centerline_path.points, start_pose); + autoware_motion_utils::findNearestSegmentIndex(centerline_path.points, start_pose); if (!start_pose_nearest_segment_index) return false; const auto start_pose_point_msg = autoware_universe_utils::createPoint( start_pose.position.x, start_pose.position.y, start_pose.position.z); - const auto starting_pose_lateral_offset = motion_utils::calcLateralOffset( + const auto starting_pose_lateral_offset = autoware_motion_utils::calcLateralOffset( centerline_path.points, start_pose_point_msg, start_pose_nearest_segment_index.value()); if (std::isnan(starting_pose_lateral_offset)) return false; @@ -505,7 +505,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const std::for_each( target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { - const auto arc_length = motion_utils::calcSignedArcLength( + const auto arc_length = autoware_motion_utils::calcSignedArcLength( centerline_path.points, current_pose.position, o.initial_pose.pose.position); if (arc_length > 0.0) return; if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; @@ -684,10 +684,10 @@ BehaviorModuleOutput StartPlannerModule::plan() const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { - const double start_distance = motion_utils::calcSignedArcLength( + const double start_distance = autoware_motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); - const double finish_distance = motion_utils::calcSignedArcLength( + const double finish_distance = autoware_motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); @@ -698,7 +698,7 @@ BehaviorModuleOutput StartPlannerModule::plan() setDebugData(); return output; } - const double distance = motion_utils::calcSignedArcLength( + const double distance = autoware_motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); @@ -788,10 +788,10 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { - const double start_distance = motion_utils::calcSignedArcLength( + const double start_distance = autoware_motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); - const double finish_distance = motion_utils::calcSignedArcLength( + const double finish_distance = autoware_motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); @@ -803,7 +803,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() return output; } - const double distance = motion_utils::calcSignedArcLength( + const double distance = autoware_motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); @@ -1295,9 +1295,9 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const Pose & current_pose = planner_data_->self_odometry->pose.pose; const auto shift_start_idx = - motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position); + autoware_motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position); const auto shift_end_idx = - motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position); + autoware_motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position); const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); const auto is_ignore_signal = [this](const lanelet::Id & id) { @@ -1336,8 +1336,9 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() } constexpr double distance_threshold = 1.0; const auto stop_point = status_.pull_out_path.partial_paths.front().points.back(); - const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength( - path.points, stop_point.point.pose.position, current_pose.position)); + const double distance_from_ego_to_stop_point = + std::abs(autoware_motion_utils::calcSignedArcLength( + path.points, stop_point.point.pose.position, current_pose.position)); return distance_from_ego_to_stop_point < distance_threshold; }); @@ -1449,7 +1450,7 @@ bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const // Return true when the goal is located behind of ego. const auto ego_lane_path = rh->getCenterLinePath( lanelet::ConstLanelets{ego_lanelet}, 0.0, std::numeric_limits::max()); - const auto dist_ego_to_goal = motion_utils::calcSignedArcLength( + const auto dist_ego_to_goal = autoware_motion_utils::calcSignedArcLength( ego_lane_path.points, getEgoPosition(), rh->getGoalPose().position); const bool is_goal_behind_of_ego = (dist_ego_to_goal < 0.0); @@ -1618,7 +1619,7 @@ void StartPlannerModule::setDebugData() {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}}; double collision_check_distance_from_end = collision_check_distances[status_.planner_type]; - const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + const auto collision_check_end_pose = autoware_motion_utils::calcLongitudinalOffsetPose( getFullPath().points, status_.pull_out_path.end_pose.position, collision_check_distance_from_end); if (collision_check_end_pose) { @@ -1672,10 +1673,10 @@ void StartPlannerModule::setDebugData() PathWithLaneId path_shift_start_to_end{}; const auto shift_path = status_.pull_out_path.partial_paths.front(); { - const size_t pull_out_start_idx = motion_utils::findNearestIndex( + const size_t pull_out_start_idx = autoware_motion_utils::findNearestIndex( shift_path.points, status_.pull_out_path.start_pose.position); - const size_t pull_out_end_idx = - motion_utils::findNearestIndex(shift_path.points, status_.pull_out_path.end_pose.position); + const size_t pull_out_end_idx = autoware_motion_utils::findNearestIndex( + shift_path.points, status_.pull_out_path.end_pose.position); path_shift_start_to_end.points.insert( path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp index ef0219b89379..82eadd656ac3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/util.cpp @@ -18,9 +18,9 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include #include -#include #include #include @@ -115,16 +115,16 @@ std::optional extractCollisionCheckSection( if (full_path.points.empty()) return std::nullopt; // Find the start index for collision check section based on the shift start pose const auto shift_start_idx = - motion_utils::findNearestIndex(full_path.points, path.start_pose.position); + autoware_motion_utils::findNearestIndex(full_path.points, path.start_pose.position); // Find the end index for collision check section based on the end pose and collision check // distance const auto collision_check_end_idx = [&]() -> size_t { - const auto end_pose_offset = motion_utils::calcLongitudinalOffsetPose( + const auto end_pose_offset = autoware_motion_utils::calcLongitudinalOffsetPose( full_path.points, path.end_pose.position, collision_check_distance_from_end); return end_pose_offset - ? motion_utils::findNearestIndex(full_path.points, end_pose_offset->position) + ? autoware_motion_utils::findNearestIndex(full_path.points, end_pose_offset->position) : full_path.points.size() - 1; // Use the last point if offset pose is not calculable }(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index 78c74dbf3706..04870464c277 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -177,14 +177,14 @@ class AvoidanceHelper double getShift(const Point & p) const { validate(); - const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p); + const auto idx = autoware_motion_utils::findNearestIndex(prev_reference_path_.points, p); return prev_spline_shift_path_.shift_length.at(idx); } double getLinearShift(const Point & p) const { validate(); - const auto idx = motion_utils::findNearestIndex(prev_reference_path_.points, p); + const auto idx = autoware_motion_utils::findNearestIndex(prev_reference_path_.points, p); return prev_linear_shift_path_.shift_length.at(idx); } @@ -278,7 +278,7 @@ class AvoidanceHelper } const auto start_idx = data_->findEgoIndex(path.points); - const auto distance = motion_utils::calcSignedArcLength( + const auto distance = autoware_motion_utils::calcSignedArcLength( path.points, start_idx, max_v_point_.value().first.position); return std::make_pair(distance, max_v_point_.value().second); } @@ -290,7 +290,7 @@ class AvoidanceHelper const auto & a_now = data_->self_acceleration->accel.accel.linear.x; const auto & a_lim = use_hard_constraints ? p->max_deceleration : p->nominal_deceleration; const auto & j_lim = use_hard_constraints ? p->max_jerk : p->nominal_jerk; - const auto ret = motion_utils::calcDecelDistWithJerkAndAccConstraints( + const auto ret = autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints( getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim); if (!!ret) { @@ -449,15 +449,15 @@ class AvoidanceHelper const auto x_max_accel = v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0; - const auto point = motion_utils::calcLongitudinalOffsetPose( + const auto point = autoware_motion_utils::calcLongitudinalOffsetPose( path.points, getEgoPosition(), x_neg_jerk + x_max_accel); if (point.has_value()) { max_v_point_ = std::make_pair(point.value(), v_max); return; } - const auto x_end = - motion_utils::calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1); + const auto x_end = autoware_motion_utils::calcSignedArcLength( + path.points, getEgoPosition(), path.points.size() - 1); const auto t_end = (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration; const auto v_end = v0 + p->max_acceleration * t_end; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 18c6ff9a6494..ecd83c31f32c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -115,8 +115,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const auto ego_idx = planner_data_->findEgoIndex(path.points); for (const auto & left_shift : left_shift_array_) { - const double start_distance = - motion_utils::calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position); + const double start_distance = autoware_motion_utils::calcSignedArcLength( + path.points, ego_idx, left_shift.start_pose.position); const double finish_distance = start_distance + left_shift.relative_longitudinal; rtc_interface_ptr_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); @@ -128,8 +128,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } for (const auto & right_shift : right_shift_array_) { - const double start_distance = - motion_utils::calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position); + const double start_distance = autoware_motion_utils::calcSignedArcLength( + path.points, ego_idx, right_shift.start_pose.position); const double finish_distance = start_distance + right_shift.relative_longitudinal; rtc_interface_ptr_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp index 5b391eb83d0a..0ac860fc4188 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#include +#include +#include #include #include #include #include -#include -#include -#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index 4284717f181e..ea6d405f80f4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_motion_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_route_handler @@ -32,7 +33,6 @@ geometry_msgs lanelet2_extension magic_enum - motion_utils pluginlib rclcpp sensor_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 1cd3db974b83..6600846ce04a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -278,7 +278,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( // arclength from ego pose (used in many functions) data.arclength_from_ego = utils::calcPathArcLengthArray( data.reference_path, 0, data.reference_path.points.size(), - motion_utils::calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); + autoware_motion_utils::calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); data.to_return_point = utils::static_obstacle_avoidance::calcDistanceToReturnDeadLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); @@ -378,7 +378,7 @@ ObjectData StaticObstacleAvoidanceModule::createObjectData( const auto & path_points = data.reference_path.points; const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - motion_utils::findNearestIndex(path_points, object_pose.position); + autoware_motion_utils::findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; const auto object_type = utils::getHighestProbLabel(object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); @@ -424,7 +424,7 @@ bool StaticObstacleAvoidanceModule::canYieldManeuver(const AvoidancePlanningData const auto registered_lines = path_shifter_.getShiftLines(); if (!registered_lines.empty()) { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); - const auto prepare_distance = motion_utils::calcSignedArcLength( + const auto prepare_distance = autoware_motion_utils::calcSignedArcLength( path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); if (!helper_->isEnoughPrepareDistance(prepare_distance)) { RCLCPP_DEBUG( @@ -642,7 +642,7 @@ void StaticObstacleAvoidanceModule::fillDebugData( const auto prepare_distance = helper_->getNominalPrepareDistance(); const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance; - dead_pose_ = motion_utils::calcLongitudinalOffsetPose( + dead_pose_ = autoware_motion_utils::calcLongitudinalOffsetPose( data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); if (!dead_pose_) { @@ -812,7 +812,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( return a.start_idx < b.start_idx; }); return std::max( - max_dist, motion_utils::calcSignedArcLength( + max_dist, autoware_motion_utils::calcSignedArcLength( previous_path.points, lines.front().start.position, getEgoPosition())); }(); @@ -821,7 +821,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin); const size_t orig_ego_idx = planner_data_->findEgoIndex(original_path.points); - const auto prev_ego_idx = motion_utils::findNearestSegmentIndex( + const auto prev_ego_idx = autoware_motion_utils::findNearestSegmentIndex( previous_path.points, getPose(original_path.points.at(orig_ego_idx)), std::numeric_limits::max(), planner_data_->parameters.ego_nearest_yaw_threshold); if (!prev_ego_idx) { @@ -832,7 +832,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( for (size_t i = 0; i < prev_ego_idx; ++i) { if ( backward_length > - motion_utils::calcSignedArcLength(previous_path.points, clip_idx, *prev_ego_idx)) { + autoware_motion_utils::calcSignedArcLength(previous_path.points, clip_idx, *prev_ego_idx)) { break; } clip_idx = i; @@ -925,7 +925,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() constexpr double threshold = 1.0; const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); const auto lateral_deviation = - motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); + autoware_motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); return std::abs(lateral_deviation) > threshold; }; @@ -1471,7 +1471,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( // Consider the difference in path length between the shifted path and original path (the path // that is shifted inward has a shorter distance to the end of the path than the other one.) const auto & to_reference_path_end = data.arclength_from_ego.back(); - const auto to_shifted_path_end = motion_utils::calcSignedArcLength( + const auto to_shifted_path_end = autoware_motion_utils::calcSignedArcLength( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); @@ -1509,7 +1509,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { const auto distance_from_ego = - motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); + autoware_motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); // slow down speed is inserted only in front of the object. const auto shift_longitudinal_distance = to_stop_line - distance_from_ego; @@ -1602,8 +1602,8 @@ void StaticObstacleAvoidanceModule::insertStopPoint( return shifted_path.path.points.size() - 1; }(); - const auto stop_distance = - motion_utils::calcSignedArcLength(shifted_path.path.points, getEgoPosition(), stop_idx); + const auto stop_distance = autoware_motion_utils::calcSignedArcLength( + shifted_path.path.points, getEgoPosition(), stop_idx); // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately @@ -1715,7 +1715,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { const auto distance_from_ego = - motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); + autoware_motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); // slow down speed is inserted only in front of the object. const auto shift_longitudinal_distance = distance_to_object - distance_from_ego; @@ -1732,7 +1732,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert); } - slow_pose_ = motion_utils::calcLongitudinalOffsetPose( + slow_pose_ = autoware_motion_utils::calcLongitudinalOffsetPose( shifted_path.path.points, start_idx, distance_to_object); } @@ -1759,7 +1759,7 @@ void StaticObstacleAvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifte const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { const auto distance_from_ego = - motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); + autoware_motion_utils::calcSignedArcLength(shifted_path.path.points, start_idx, i); // slow down speed is inserted only in front of the object. const auto accel_distance = distance_to_accel_end_point - distance_from_ego; @@ -1779,7 +1779,7 @@ void StaticObstacleAvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifte shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_target); } - slow_pose_ = motion_utils::calcLongitudinalOffsetPose( + slow_pose_ = autoware_motion_utils::calcLongitudinalOffsetPose( shifted_path.path.points, start_idx, distance_to_accel_end_point); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 634ee0e71d73..7580623cff71 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -355,8 +355,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( return false; } const auto goal_pose = data_->route_handler->getGoalPose(); - const double goal_longitudinal_distance = - motion_utils::calcSignedArcLength(data.reference_path.points, 0, goal_pose.position); + const double goal_longitudinal_distance = autoware_motion_utils::calcSignedArcLength( + data.reference_path.points, 0, goal_pose.position); const bool is_return_shift_to_goal = std::abs(al_return.end_longitudinal - goal_longitudinal_distance) < parameters_->object_check_return_pose_distance; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 71935f502dd0..2704829ad6a1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -68,7 +68,7 @@ geometry_msgs::msg::Polygon toMsg( template size_t findFirstNearestIndex(const T & points, const geometry_msgs::msg::Point & point) { - motion_utils::validateNonEmpty(points); + autoware_motion_utils::validateNonEmpty(points); double min_dist = std::numeric_limits::max(); size_t min_idx = 0; @@ -104,7 +104,7 @@ size_t findFirstNearestSegmentIndex(const T & points, const geometry_msgs::msg:: } const double signed_length = - motion_utils::calcLongitudinalOffsetToSegment(points, nearest_idx, point); + autoware_motion_utils::calcLongitudinalOffsetToSegment(points, nearest_idx, point); if (signed_length <= 0) { return nearest_idx - 1; @@ -119,7 +119,7 @@ double calcSignedArcLengthToFirstNearestPoint( const geometry_msgs::msg::Point & dst_point) { try { - motion_utils::validateNonEmpty(points); + autoware_motion_utils::validateNonEmpty(points); } catch (const std::exception & e) { std::cerr << e.what() << std::endl; return 0.0; @@ -129,11 +129,11 @@ double calcSignedArcLengthToFirstNearestPoint( const size_t dst_seg_idx = findFirstNearestSegmentIndex(points, dst_point); const double signed_length_on_traj = - motion_utils::calcSignedArcLength(points, src_seg_idx, dst_seg_idx); + autoware_motion_utils::calcSignedArcLength(points, src_seg_idx, dst_seg_idx); const double signed_length_src_offset = - motion_utils::calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + autoware_motion_utils::calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); const double signed_length_dst_offset = - motion_utils::calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + autoware_motion_utils::calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } @@ -759,7 +759,7 @@ bool isSatisfiedWithCommonCondition( const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points); const auto to_goal_distance = rh->isInGoalRouteSection(data.current_lanelets.back()) - ? motion_utils::calcSignedArcLength( + ? autoware_motion_utils::calcSignedArcLength( data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1) : std::numeric_limits::max(); @@ -946,7 +946,7 @@ double getRoadShoulderDistance( const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - motion_utils::findNearestIndex(data.reference_path.points, object_pose.position); + autoware_motion_utils::findNearestIndex(data.reference_path.points, object_pose.position); const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; const auto rh = planner_data->route_handler; @@ -1195,7 +1195,8 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( for (const auto & p : obj.envelope_poly.outer()) { const auto point = autoware_universe_utils::createPoint(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. - const double arc_length = motion_utils::calcSignedArcLength(path.points, ego_pos, point); + const double arc_length = + autoware_motion_utils::calcSignedArcLength(path.points, ego_pos, point); min_distance = std::min(min_distance, arc_length); max_distance = std::max(max_distance, arc_length); } @@ -1212,7 +1213,7 @@ std::vector> calcEnvelopeOverhangDistance( for (const auto & p : object_data.envelope_poly.outer()) { const auto point = autoware_universe_utils::createPoint(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. - const auto idx = motion_utils::findNearestIndex(path.points, point); + const auto idx = autoware_motion_utils::findNearestIndex(path.points, point); const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); overhang_points.emplace_back(lateral, point); } @@ -1383,16 +1384,18 @@ void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, std::optional & p_out) { - const auto decel_point = motion_utils::calcLongitudinalOffsetPoint(path.points, p_src, offset); + const auto decel_point = + autoware_motion_utils::calcLongitudinalOffsetPoint(path.points, p_src, offset); if (!decel_point) { // TODO(Satoshi OTA) Think later the process in the case of no decel point found. return; } - const auto seg_idx = motion_utils::findNearestSegmentIndex(path.points, decel_point.value()); + const auto seg_idx = + autoware_motion_utils::findNearestSegmentIndex(path.points, decel_point.value()); const auto insert_idx = - motion_utils::insertTargetPoint(seg_idx, decel_point.value(), path.points); + autoware_motion_utils::insertTargetPoint(seg_idx, decel_point.value(), path.points); if (!insert_idx) { // TODO(Satoshi OTA) Think later the process in the case of no decel point found. @@ -1814,9 +1817,9 @@ void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineAr // calc longitudinal for (auto & sl : lines) { - sl.start_idx = motion_utils::findNearestIndex(path.points, sl.start.position); + sl.start_idx = autoware_motion_utils::findNearestIndex(path.points, sl.start.position); sl.start_longitudinal = arc.at(sl.start_idx); - sl.end_idx = motion_utils::findNearestIndex(path.points, sl.end.position); + sl.end_idx = autoware_motion_utils::findNearestIndex(path.points, sl.end.position); sl.end_longitudinal = arc.at(sl.end_idx); } } @@ -2114,7 +2117,7 @@ std::pair separateObjectsByPath( double next_longitudinal_distance = parameters->resample_interval_for_output; for (size_t i = 0; i < points_size; ++i) { const auto distance_from_ego = - motion_utils::calcSignedArcLength(reference_path.points, ego_idx, i); + autoware_motion_utils::calcSignedArcLength(reference_path.points, ego_idx, i); if (distance_from_ego > object_check_forward_distance) { break; } @@ -2362,7 +2365,7 @@ double calcDistanceToReturnDeadLine( if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { const auto & ego_pos = planner_data->self_odometry->pose.pose.position; const auto to_goal_distance = - motion_utils::calcSignedArcLength(path.points, ego_pos, path.points.size() - 1); + autoware_motion_utils::calcSignedArcLength(path.points, ego_pos, path.points.size() - 1); distance_to_return_dead_line = std::min( distance_to_return_dead_line, to_goal_distance - parameters->dead_line_buffer_for_goal); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index cff7d952c901..dd4858dabe29 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -17,13 +17,13 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler autoware_universe_utils geometry_msgs lanelet2_extension - motion_utils pluginlib rclcpp tf2 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp index b881447e837f..31d90a7d7d70 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/debug.cpp @@ -16,8 +16,8 @@ #include #include +#include #include -#include #include @@ -73,12 +73,12 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( } // namespace -motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWalls virtual_walls; if (debug_data_.virtual_wall_pose) { - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWall wall; wall.text = "blind_spot"; wall.pose = debug_data_.virtual_wall_pose.value(); wall.ns = std::to_string(module_id_) + "_"; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 6b963d86c9bb..e4b3a341a9b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include namespace autoware::behavior_velocity_planner { @@ -88,8 +88,8 @@ void BlindSpotModule::setRTCStatusByDecision( { setSafe(false); const auto & current_pose = planner_data_->current_odometry->pose; - setDistance( - motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + setDistance(autoware_motion_utils::calcSignedArcLength( + path.points, current_pose.position, decision.stop_line_idx)); return; } @@ -124,8 +124,8 @@ void BlindSpotModule::setRTCStatusByDecision( { setSafe(true); const auto & current_pose = planner_data_->current_odometry->pose; - setDistance( - motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + setDistance(autoware_motion_utils::calcSignedArcLength( + path.points, current_pose.position, decision.stop_line_idx)); return; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index f651b25994da..e51a55784b72 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -17,10 +17,10 @@ #include #include #include +#include #include #include #include -#include #include #include @@ -91,7 +91,7 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( /* calc closest index */ const auto & current_pose = planner_data_->current_odometry->pose; - const auto closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto closest_idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); const auto stop_line_idx = @@ -299,7 +299,7 @@ static std::optional insertPointIndex( return duplicate_idx_opt.value(); } - const size_t closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t closest_idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) @@ -361,12 +361,12 @@ std::optional> BlindSpotModule::generateStopLine( const geometry_msgs::msg::Point mid_point = geometry_msgs::build().x(mid_pt.x()).y(mid_pt.y()).z(mid_pt.z()); stop_idx_default_ip = stop_idx_critical_ip = - motion_utils::findNearestSegmentIndex(path_ip.points, mid_point); + autoware_motion_utils::findNearestSegmentIndex(path_ip.points, mid_point); /* // NOTE: it is not ambiguous when the curve starts on the left/right lanelet, so this module inserts stopline at the beginning of the lanelet for baselink stop_idx_default_ip = stop_idx_critical_ip = static_cast(std::max(0, - static_cast(motion_utils::findNearestSegmentIndex(path_ip.points, mid_point)) - + static_cast(autoware_motion_utils::findNearestSegmentIndex(path_ip.points, mid_point)) - baselink2front_dist)); */ } @@ -419,12 +419,13 @@ std::optional BlindSpotModule::isOverPassJudge( const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithAccLimit( planner_data_->current_velocity->twist.linear.x, planner_data_->max_stop_acceleration_threshold, planner_data_->delay_response_time); - const auto ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); + const auto ego_segment_idx = + autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); const size_t stop_point_segment_idx = - motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); - const auto distance_until_stop = motion_utils::calcSignedArcLength( + autoware_motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); + const auto distance_until_stop = autoware_motion_utils::calcSignedArcLength( input_path.points, current_pose.position, ego_segment_idx, stop_point_pose.position, stop_point_segment_idx); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp index 981c94f7ce42..8c4cae6ba399 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp @@ -117,7 +117,7 @@ class BlindSpotModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - std::vector createVirtualWalls() override; + std::vector createVirtualWalls() override; private: // (semi) const variables diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index 420ff10b6fb5..85a19411405a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_velocity_planner_common autoware_grid_map_utils + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -35,7 +36,6 @@ interpolation lanelet2_extension libboost-dev - motion_utils nav_msgs pcl_conversions pluginlib diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp index 8b3d2029a4cb..8e8686e984a4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp @@ -15,23 +15,23 @@ #include "scene_crosswalk.hpp" #include +#include #include #include -#include #include namespace autoware::behavior_velocity_planner { +using autoware_motion_utils::createSlowDownVirtualWallMarker; +using autoware_motion_utils::createStopVirtualWallMarker; using autoware_universe_utils::appendMarkerArray; using autoware_universe_utils::calcOffsetPose; using autoware_universe_utils::createDefaultMarker; using autoware_universe_utils::createMarkerColor; using autoware_universe_utils::createMarkerScale; using autoware_universe_utils::createPoint; -using motion_utils::createSlowDownVirtualWallMarker; -using motion_utils::createStopVirtualWallMarker; using visualization_msgs::msg::Marker; namespace @@ -177,19 +177,19 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( } } // namespace -motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; wall.text = "crosswalk"; wall.ns = std::to_string(module_id_) + "_"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware_motion_utils::VirtualWallType::slowdown; wall.text += debug_data_.virtual_wall_suffix; for (const auto & p : debug_data_.slow_poses) { wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 2ee7ff186fcb..631f08ce8494 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -18,12 +18,12 @@ #include #include +#include +#include +#include #include #include #include -#include -#include -#include #include #include @@ -41,22 +41,22 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; +using autoware_motion_utils::calcArcLength; +using autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints; +using autoware_motion_utils::calcLateralOffset; +using autoware_motion_utils::calcLongitudinalOffsetPoint; +using autoware_motion_utils::calcLongitudinalOffsetPose; +using autoware_motion_utils::calcSignedArcLength; +using autoware_motion_utils::calcSignedArcLengthPartialSum; +using autoware_motion_utils::findNearestSegmentIndex; +using autoware_motion_utils::insertTargetPoint; +using autoware_motion_utils::resamplePath; using autoware_universe_utils::createPoint; using autoware_universe_utils::getPose; using autoware_universe_utils::Point2d; using autoware_universe_utils::Polygon2d; using autoware_universe_utils::pose2transform; using autoware_universe_utils::toHexString; -using motion_utils::calcArcLength; -using motion_utils::calcDecelDistWithJerkAndAccConstraints; -using motion_utils::calcLateralOffset; -using motion_utils::calcLongitudinalOffsetPoint; -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcSignedArcLength; -using motion_utils::calcSignedArcLengthPartialSum; -using motion_utils::findNearestSegmentIndex; -using motion_utils::insertTargetPoint; -using motion_utils::resamplePath; namespace { @@ -385,7 +385,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const double dist_ego2crosswalk = calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()); - const auto braking_distance_opt = motion_utils::calcDecelDistWithJerkAndAccConstraints( + const auto braking_distance_opt = autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints( ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, p.min_jerk_for_no_stop_decision); const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index b57c2bc4d66a..8d53b0a1fe4f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -327,7 +327,7 @@ class CrosswalkModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; private: // main functions diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp index 08f741103c8a..c2d9f8273cab 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp @@ -15,11 +15,11 @@ #include "autoware/behavior_velocity_crosswalk_module/util.hpp" #include +#include +#include #include #include #include -#include -#include #include @@ -48,10 +48,10 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; +using autoware_motion_utils::calcSignedArcLength; using autoware_universe_utils::createPoint; using autoware_universe_utils::Line2d; using autoware_universe_utils::Point2d; -using motion_utils::calcSignedArcLength; std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index 982246ffed49..5a15c78c9dee 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -18,6 +18,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_planning_msgs autoware_route_handler autoware_universe_utils @@ -25,7 +26,6 @@ geometry_msgs lanelet2_extension libboost-dev - motion_utils pluginlib rclcpp tf2 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp index 2431c871633b..4eff964f96c6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/debug.cpp @@ -16,9 +16,9 @@ #include #include +#include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -171,19 +171,19 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray return wall_marker; } -motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; wall.text = "detection_area"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } - wall.style = motion_utils::VirtualWallType::deadline; + wall.style = autoware_motion_utils::VirtualWallType::deadline; for (const auto & p : debug_data_.dead_line_poses) { wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 827470317180..954bbc988b1c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -33,8 +33,8 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcSignedArcLength; +using autoware_motion_utils::calcLongitudinalOffsetPose; +using autoware_motion_utils::calcSignedArcLength; DetectionAreaModule::DetectionAreaModule( const int64_t module_id, const int64_t lane_id, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index d84a3559f373..595ab663f8b0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -73,7 +73,7 @@ class DetectionAreaModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; private: LineString2d getStopLineGeometry2d() const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index c2b5ed9d3cfe..e240722e66a5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -19,6 +19,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -31,7 +32,6 @@ lanelet2_extension libopencv-dev magic_enum - motion_utils nav_msgs pluginlib rclcpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp index baf7aa806c3d..1543966e4727 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp @@ -17,8 +17,8 @@ #include #include +#include #include -#include #include @@ -424,27 +424,27 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( return debug_marker_array; } -motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; if (debug_data_.collision_stop_wall_pose) { - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.collision_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_first_stop_wall_pose) { - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection_occlusion_first_stop" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_first_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_stop_wall_pose) { - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; wall.text = "intersection_occlusion"; if (debug_data_.static_occlusion_with_traffic_light_timeout) { std::stringstream timeout; @@ -457,7 +457,7 @@ motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() virtual_walls.push_back(wall); } if (debug_data_.absence_traffic_light_creep_wall) { - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware_motion_utils::VirtualWallType::slowdown; wall.text = "intersection_occlusion"; wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.absence_traffic_light_creep_wall.value(); @@ -483,13 +483,13 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark return debug_marker_array; } -motion_utils::VirtualWalls MergeFromPrivateRoadModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls MergeFromPrivateRoadModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWalls virtual_walls; const auto state = state_machine_.getState(); if (state == StateMachine::State::STOP) { - motion_utils::VirtualWall wall; - wall.style = motion_utils::VirtualWallType::stop; + autoware_motion_utils::VirtualWall wall; + wall.style = autoware_motion_utils::VirtualWallType::stop; wall.pose = debug_data_.virtual_wall_pose; wall.text = "merge_from_private_road"; virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index b7c55dffec5d..3a760d404c80 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -18,13 +18,13 @@ #include // for toGeomPoly #include +#include +#include #include // for toPolygon2d #include #include #include #include -#include -#include #include @@ -42,7 +42,7 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using motion_utils::VelocityFactorInterface; +using autoware_motion_utils::VelocityFactorInterface; IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, @@ -342,7 +342,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail( // Occluded // utility functions auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + return autoware_motion_utils::calcSignedArcLength(path->points, closest_idx, index); }; auto stoppedForDuration = [&](const size_t pos, const double duration, StateMachine & state_machine) { @@ -527,12 +527,13 @@ void prepareRTCByDecisionResult( const auto closest_idx = result.closest_idx; const auto stopline_idx = result.stuck_stopline_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); + *default_distance = + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); *occlusion_safety = true; if (result.occlusion_stopline_idx) { const auto occlusion_stopline_idx = result.occlusion_stopline_idx.value(); *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); } return; } @@ -547,7 +548,8 @@ void prepareRTCByDecisionResult( const auto closest_idx = result.closest_idx; const auto stopline_idx = result.stuck_stopline_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); + *default_distance = + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); *occlusion_safety = true; return; } @@ -563,11 +565,11 @@ void prepareRTCByDecisionResult( const auto collision_stopline_idx = result.collision_stopline_idx; *default_safety = false; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); const auto occlusion_stopline = result.occlusion_stopline_idx; *occlusion_safety = true; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline); return; } @@ -583,10 +585,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = false; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, first_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, first_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -602,10 +604,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -620,7 +622,7 @@ void prepareRTCByDecisionResult( const auto collision_stopline_idx = result.closest_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = 0; return; @@ -638,10 +640,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = false; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -656,10 +658,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = true; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -675,10 +677,10 @@ void prepareRTCByDecisionResult( const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = true; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index 9829f93362e3..6aa81162bea2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include @@ -325,7 +325,7 @@ class IntersectionModule : public SceneModuleInterface /** @}*/ visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; const std::set & getAssociativeIds() const { return associative_ids_; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 6e0fc2958d4f..041715b2dc1a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -17,11 +17,11 @@ #include // for toGeomPoly #include // for smoothPath +#include #include // for toPolygon2d #include #include #include -#include #include #include @@ -630,7 +630,7 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( object_dist_to_margin_point / std::max(min_vel, object_info->observed_velocity()); // ego side const double ego_dist_to_collision_pos = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_pos); + autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, collision_pos); const auto ego_eta_to_collision_pos_it = std::lower_bound( ego_time_distance_array.begin(), ego_time_distance_array.end(), ego_dist_to_collision_pos, [](const auto & a, const double b) { return a.second < b; }); @@ -897,15 +897,16 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so // `last_intersection_stopline_candidate_idx` makes no sense - const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, path.points.at(closest_idx).point.pose, - planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + const auto smoothed_path_closest_idx = + autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, path.points.at(closest_idx).point.pose, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { if (upstream_stopline) { const auto upstream_stopline_point = reference_path.points.at(upstream_stopline.value()).point.pose; - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( smoothed_reference_path.points, upstream_stopline_point, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); } else { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 657276f00ae8..42d91defe756 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -17,11 +17,11 @@ #include // for to_bg2d #include // for planning_utils:: +#include #include #include // for lanelet::autoware::RoadMarking #include #include -#include #include #include @@ -198,7 +198,7 @@ Result IntersectionModule::prepare const auto & path_ip = interpolated_path_info.path; const auto & path_ip_intersection_end = interpolated_path_info.lane_id_interval.value().second; - internal_debug_data_.distance = motion_utils::calcSignedArcLength( + internal_debug_data_.distance = autoware_motion_utils::calcSignedArcLength( path->points, current_pose.position, path_ip.points.at(path_ip_intersection_end).point.pose.position); @@ -275,7 +275,7 @@ Result IntersectionModule::prepare if (is_green_solid_on && !initial_green_light_observed_time_) { const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); const bool approached_assigned_lane = - motion_utils::calcSignedArcLength( + autoware_motion_utils::calcSignedArcLength( path->points, closest_idx, autoware_universe_utils::createPoint( assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), @@ -338,7 +338,7 @@ std::optional IntersectionModule::getStopLineIndexFromMap( stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); } @@ -413,7 +413,7 @@ std::optional IntersectionModule::generateIntersectionSto // (2) ego front stop line position on interpolated path const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; - const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( + const auto closest_idx_ip = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 4d13890be5c6..124f679e00a5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -16,8 +16,8 @@ #include "util.hpp" #include // for toGeomPoly +#include #include -#include #include #include @@ -124,7 +124,7 @@ std::optional IntersectionModule::isStuckStatus( { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path.points, closest_idx, index); + return autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, index); }; const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK @@ -310,7 +310,7 @@ std::optional IntersectionModule::isYieldStuckStatus( { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path.points, closest_idx, index); + return autoware_motion_utils::calcSignedArcLength(path.points, closest_idx, index); }; const auto & intersection_lanelets = intersection_lanelets_.value(); const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index f675d24e5e16..dcbd41eccaec 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -18,9 +18,9 @@ #include #include +#include #include #include -#include #include #include @@ -159,7 +159,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); - const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( + const double signed_arc_dist_to_stop_point = autoware_motion_utils::calcSignedArcLength( path->points, current_pose.position, path->points.at(stopline_idx).point.pose.position); if ( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index 118957fc8fd3..4960c7ab9e8d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -70,7 +70,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; const std::set & getAssociativeIds() const { return associative_ids_; } lanelet::ConstLanelets getAttentionLanelets() const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp index 85751442589f..e56b99486c93 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -19,8 +19,8 @@ #include #include #include +#include #include -#include #include #include @@ -68,7 +68,7 @@ std::optional insertPointIndex( return duplicate_idx_opt.value(); } - const size_t closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t closest_idx = autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index df64b5d8a4f6..eab1a13d7ebf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -18,6 +18,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -28,7 +29,6 @@ interpolation lanelet2_extension libboost-dev - motion_utils pluginlib rclcpp tf2 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp index 488132113d2d..b587036d9027 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -16,9 +16,9 @@ #include #include +#include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -162,13 +162,13 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createDebugMarkerArra return debug_marker_array; } -motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; wall.ns = std::to_string(module_id_) + "_"; wall.text = "no_stopping_area"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 7ece8dd8ace9..18169dfa1ba7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -17,10 +17,10 @@ #include #include #include +#include #include #include #include -#include #include #include @@ -132,7 +132,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason return true; } const auto & stop_pose = stop_point->second; - setDistance(motion_utils::calcSignedArcLength( + setDistance(autoware_motion_utils::calcSignedArcLength( original_path.points, current_pose->pose.position, stop_pose.position)); if (planning_utils::isOverLine( original_path, current_pose->pose, stop_pose, planner_param_.dead_line_margin)) { @@ -266,7 +266,7 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( } // judge if stop point p0 is near goal, by its distance to the path end. const double dist_to_path_end = - motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1); + autoware_motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1); if (dist_to_path_end < close_to_goal_distance) { // exit with false, cause position is near goal. return false; @@ -302,10 +302,10 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( auto & pp = interpolated_path.points; /* calc closest index */ const auto closest_idx_opt = - motion_utils::findNearestIndex(interpolated_path.points, ego_pose, 3.0, M_PI_4); + autoware_motion_utils::findNearestIndex(interpolated_path.points, ego_pose, 3.0, M_PI_4); if (!closest_idx_opt) { RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, *clock_, 1000 /* ms */, "motion_utils::findNearestIndex fail"); + logger_, *clock_, 1000 /* ms */, "autoware_motion_utils::findNearestIndex fail"); return ego_area; } const size_t closest_idx = closest_idx_opt.value(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 103eb968e859..eead30b11f53 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -86,7 +86,7 @@ class NoStoppingAreaModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; private: const int64_t lane_id_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml index 116fb72176a3..34fd712d2229 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -18,6 +18,7 @@ autoware_behavior_velocity_planner_common autoware_grid_map_utils + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -29,7 +30,6 @@ lanelet2_extension libboost-dev libopencv-dev - motion_utils nav_msgs pluginlib rclcpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp index 6a05178ee769..dff8ce1da909 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -17,8 +17,8 @@ #include #include +#include #include -#include #include #include @@ -214,12 +214,12 @@ MarkerArray OcclusionSpotModule::createDebugMarkerArray() return debug_marker_array; } -motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; wall.text = "occlusion_spot"; - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware_motion_utils::VirtualWallType::slowdown; for (size_t id = 0; id < debug_data_.debug_poses.size(); id++) { wall.pose = calcOffsetPose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index d9ff13946c97..189a5be5abe9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -18,10 +18,10 @@ #include "grid_utils.hpp" #include +#include #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 90c7a37e6420..27e3452832b5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -110,12 +110,12 @@ bool OcclusionSpotModule::modifyPathVelocity( //! never change this interpolation interval(will affect module accuracy) splineInterpolate(clipped_path, 1.0, path_interpolated, logger_); const geometry_msgs::msg::Point start_point = path_interpolated.points.at(0).point.pose.position; - const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( + const auto ego_segment_idx = autoware_motion_utils::findNearestSegmentIndex( path_interpolated.points, ego_pose, param_.dist_thr, param_.angle_thr); if (!ego_segment_idx) return true; const size_t start_point_segment_idx = - motion_utils::findNearestSegmentIndex(path_interpolated.points, start_point); - const auto offset = motion_utils::calcSignedArcLength( + autoware_motion_utils::findNearestSegmentIndex(path_interpolated.points, start_point); + const auto offset = autoware_motion_utils::calcSignedArcLength( path_interpolated.points, ego_pose.position, *ego_segment_idx, start_point, start_point_segment_idx); const double offset_from_start_to_ego = -offset; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index d6f39dd56bde..4df653c26fce 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -56,7 +56,7 @@ class OcclusionSpotModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; private: // Parameter diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index 271efdc1a1b4..0c5088d7c4e1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -36,6 +36,7 @@ autoware_behavior_velocity_planner_common autoware_map_msgs + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -46,7 +47,6 @@ geometry_msgs lanelet2_extension libboost-dev - motion_utils pcl_conversions pluginlib rclcpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 9ed221f9df0b..4ca0736ce8d7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -15,12 +15,12 @@ #include "node.hpp" #include +#include +#include #include #include #include #include -#include -#include #include #include @@ -379,7 +379,7 @@ autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( autoware_planning_msgs::msg::Path output_path_msg; // TODO(someone): support backward path - const auto is_driving_forward = motion_utils::isDrivingForward(input_path_msg->points); + const auto is_driving_forward = autoware_motion_utils::isDrivingForward(input_path_msg->points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; if (!is_driving_forward_) { RCLCPP_WARN_THROTTLE( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index f9a6210d7401..e6300a2889a0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -16,13 +16,13 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ #include +#include +#include #include #include #include #include #include -#include -#include #include #include @@ -51,11 +51,11 @@ namespace autoware::behavior_velocity_planner using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::rtc_interface::RTCInterface; +using autoware_motion_utils::PlanningBehavior; +using autoware_motion_utils::VelocityFactor; using autoware_universe_utils::DebugPublisher; using autoware_universe_utils::getOrDeclareParameter; using builtin_interfaces::msg::Time; -using motion_utils::PlanningBehavior; -using motion_utils::VelocityFactor; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; @@ -87,7 +87,7 @@ class SceneModuleInterface virtual bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) = 0; virtual visualization_msgs::msg::MarkerArray createDebugMarkerArray() = 0; - virtual std::vector createVirtualWalls() = 0; + virtual std::vector createVirtualWalls() = 0; int64_t getModuleId() const { return module_id_; } void setPlannerData(const std::shared_ptr & planner_data) @@ -130,7 +130,7 @@ class SceneModuleInterface std::shared_ptr planner_data_; std::optional infrastructure_command_; std::optional first_stop_path_point_index_; - motion_utils::VelocityFactorInterface velocity_factor_; + autoware_motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; void setSafe(const bool safe) @@ -197,7 +197,7 @@ class SceneModuleManagerInterface std::set registered_module_id_set_; std::shared_ptr planner_data_; - motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_; + autoware_motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_; std::optional first_stop_path_point_index_; rclcpp::Node & node_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 96bad9b21b27..ad4b43516c5d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_map_msgs + autoware_motion_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_msgs @@ -34,7 +35,6 @@ geometry_msgs interpolation lanelet2_extension - motion_utils nav_msgs pcl_conversions rclcpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index d70d56509cc4..edc67d4bbdc9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -14,9 +14,9 @@ #include #include +#include #include #include -#include #include #include @@ -41,7 +41,7 @@ size_t SceneModuleInterface::findEgoSegmentIndex( const std::vector & points) const { const auto & p = planner_data_; - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, p->current_odometry->pose, p->ego_nearest_dist_threshold); } @@ -77,7 +77,7 @@ size_t SceneModuleManagerInterface::findEgoSegmentIndex( const std::vector & points) const { const auto & p = planner_data_; - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, p->current_odometry->pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp index a189dd52a8c9..2581edd2c827 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include -#include -#include +#include +#include #include #include @@ -34,7 +34,7 @@ bool splineInterpolate( return false; } - output = motion_utils::resamplePath(input, interval, false, true, true, false); + output = autoware_motion_utils::resamplePath(input, interval, false, true, true, false); return true; } @@ -68,7 +68,7 @@ autoware_planning_msgs::msg::Path interpolatePath( return path; } - double path_len = std::min(length, motion_utils::calcArcLength(path.points)); + double path_len = std::min(length, autoware_motion_utils::calcArcLength(path.points)); { double s = 0.0; for (size_t idx = 0; idx < path.points.size(); ++idx) { @@ -122,7 +122,7 @@ autoware_planning_msgs::msg::Path interpolatePath( return path; } - return motion_utils::resamplePath(path, s_out); + return autoware_motion_utils::resamplePath(path, s_out); } autoware_planning_msgs::msg::Path filterLitterPathPoint( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index b8c6d17e97e2..00b34fbcac61 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -13,11 +13,11 @@ // limitations under the License. // #include -#include "motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include +#include #include -#include #include #include @@ -55,7 +55,8 @@ bool smoothPath( const auto & smoother = planner_data->velocity_smoother_; auto trajectory = - motion_utils::convertToTrajectoryPoints(in_path); + autoware_motion_utils::convertToTrajectoryPoints( + in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); const auto traj_steering_rate_limited = @@ -65,9 +66,10 @@ bool smoothPath( auto traj_resampled = smoother->resampleTrajectory( traj_steering_rate_limited, v0, current_pose, planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); - const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( - traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); + const size_t traj_resampled_closest = + autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); std::vector debug_trajectories; // Clip trajectory from closest point TrajectoryPoints clipped; @@ -85,7 +87,7 @@ bool smoothPath( 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); + out_path = autoware_motion_utils::convertToPathWithLaneId(traj_smoothed); return true; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index 16a8cd1e69be..087e532b6e0d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -14,8 +14,8 @@ #include #include +#include #include -#include #include @@ -95,6 +95,9 @@ namespace autoware::behavior_velocity_planner { namespace planning_utils { +using autoware_motion_utils::calcLongitudinalOffsetToSegment; +using autoware_motion_utils::calcSignedArcLength; +using autoware_motion_utils::validateNonEmpty; using autoware_planning_msgs::msg::PathPoint; using autoware_universe_utils::calcAzimuthAngle; using autoware_universe_utils::calcDistance2d; @@ -102,9 +105,6 @@ using autoware_universe_utils::calcOffsetPose; using autoware_universe_utils::calcSquaredDistance2d; using autoware_universe_utils::createQuaternionFromYaw; using autoware_universe_utils::getPoint; -using motion_utils::calcLongitudinalOffsetToSegment; -using motion_utils::calcSignedArcLength; -using motion_utils::validateNonEmpty; size_t calcSegmentIndexFromPointIndex( const std::vector & points, @@ -120,7 +120,8 @@ size_t calcSegmentIndexFromPointIndex( return 0; } - const double offset_to_seg = motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); + const double offset_to_seg = + autoware_motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); if (0 < offset_to_seg) { return idx; } @@ -612,7 +613,8 @@ bool isOverLine( const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, const double offset) { - return motion_utils::calcSignedArcLength(path.points, self_pose.position, line_pose.position) + + return autoware_motion_utils::calcSignedArcLength( + path.points, self_pose.position, line_pose.position) + offset < 0.0; } @@ -624,9 +626,9 @@ std::optional insertDecelPoint( // TODO(tanaka): consider proper overlap threshold for inserting decel point const double overlap_threshold = 5e-2; // TODO(murooka): remove this function for u-turn and crossing-path - const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point); - const auto insert_idx = - motion_utils::insertTargetPoint(base_idx, stop_point, output.points, overlap_threshold); + const size_t base_idx = autoware_motion_utils::findNearestSegmentIndex(output.points, stop_point); + const auto insert_idx = autoware_motion_utils::insertTargetPoint( + base_idx, stop_point, output.points, overlap_threshold); if (!insert_idx) { return {}; @@ -644,8 +646,9 @@ std::optional insertDecelPoint( std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output) { - const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point); - const auto insert_idx = motion_utils::insertStopPoint(base_idx, stop_point, output.points); + const size_t base_idx = autoware_motion_utils::findNearestSegmentIndex(output.points, stop_point); + const auto insert_idx = + autoware_motion_utils::insertStopPoint(base_idx, stop_point, output.points); if (!insert_idx) { return {}; @@ -657,7 +660,8 @@ std::optional insertStopPoint( std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output) { - const auto insert_idx = motion_utils::insertStopPoint(stop_seg_idx, stop_point, output.points); + const auto insert_idx = + autoware_motion_utils::insertStopPoint(stop_seg_idx, stop_point, output.points); if (!insert_idx) { return {}; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp index fc1e8a6ea2a7..a4ee3806417f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "utils.hpp" #include @@ -84,10 +84,10 @@ TEST(smoothDeceleration, calculateMaxSlowDownVelocity) TEST(specialInterpolation, specialInterpolation) { using autoware::behavior_velocity_planner::interpolatePath; + using autoware_motion_utils::calcSignedArcLength; + using autoware_motion_utils::searchZeroVelocityIndex; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; - using motion_utils::calcSignedArcLength; - using motion_utils::searchZeroVelocityIndex; const auto genPath = [](const auto p, const auto v) { if (p.size() != v.size()) throw std::invalid_argument("different size is not expected"); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml index a1feae8c4113..ce6cf92fc645 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml @@ -21,6 +21,7 @@ autoware_behavior_velocity_crosswalk_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -30,7 +31,6 @@ geometry_msgs libboost-dev message_filters - motion_utils object_recognition_utils pcl_conversions pluginlib diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp index 9346d05128c6..bdd62fe6aa08 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -16,9 +16,9 @@ #include "scene.hpp" +#include #include #include -#include using autoware_universe_utils::appendMarkerArray; using autoware_universe_utils::calcOffsetPose; @@ -186,12 +186,12 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray return visualization_marker_array; } -motion_utils::VirtualWalls RunOutDebug::createVirtualWalls() +autoware_motion_utils::VirtualWalls RunOutDebug::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; wall.text = "run_out"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; for (const auto & p : stop_pose_) { wall.pose = p; virtual_walls.push_back(wall); @@ -363,7 +363,7 @@ visualization_msgs::msg::MarkerArray RunOutModule::createDebugMarkerArray() return debug_ptr_->createVisualizationMarkerArray(); } -motion_utils::VirtualWalls RunOutModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls RunOutModule::createVirtualWalls() { return debug_ptr_->createVirtualWalls(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp index c38dd63fadba..7aaa60e2e4b6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -16,7 +16,7 @@ #include "utils.hpp" -#include +#include #include #include @@ -123,7 +123,7 @@ class RunOutDebug void publishEmptyPointCloud(); visualization_msgs::msg::MarkerArray createVisualizationMarkerArray(); void setHeight(const double height); - motion_utils::VirtualWalls createVirtualWalls(); + autoware_motion_utils::VirtualWalls createVirtualWalls(); private: visualization_msgs::msg::MarkerArray createVisualizationMarkerArrayFromDebugData( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index fe4162504095..73e97a33ad8d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -14,12 +14,12 @@ #include "dynamic_obstacle.hpp" +#include #include #include #include #include #include -#include #include #include @@ -38,7 +38,7 @@ namespace geometry_msgs::msg::Quaternion createQuaternionFacingToTrajectory( const PathPointsWithLaneId & path_points, const geometry_msgs::msg::Point & point) { - const auto nearest_idx = motion_utils::findNearestIndex(path_points, point); + const auto nearest_idx = autoware_motion_utils::findNearestIndex(path_points, point); const auto & nearest_pose = path_points.at(nearest_idx).point.pose; const auto longitudinal_offset = @@ -166,7 +166,8 @@ std::vector> groupPointsWithNearestSegmentIndex( for (const auto & p : input_points.points) { const auto ros_point = autoware_universe_utils::createPoint(p.x, p.y, p.z); - const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(path_points, ros_point); + const size_t nearest_seg_idx = + autoware_motion_utils::findNearestSegmentIndex(path_points, ros_point); // if the point is ahead of end of the path, index should be path.size() - 1 if ( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp index 346a10646e6b..7a3083e75f25 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp @@ -14,7 +14,7 @@ #include "path_utils.hpp" -#include +#include namespace autoware::behavior_velocity_planner::run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( @@ -26,7 +26,7 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint( geometry_msgs::msg::Point min_dist_point{}; for (const auto & p : target_points) { - const float dist = motion_utils::calcSignedArcLength(points, src_point, p); + const float dist = autoware_motion_utils::calcSignedArcLength(points, src_point, p); if (dist < min_dist) { min_dist = dist; min_dist_point = p; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index 7c3f538c4253..11db6a604a30 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -19,10 +19,10 @@ #include #include +#include +#include #include #include -#include -#include #include #include @@ -272,10 +272,10 @@ std::optional RunOutModule::findNearestCollisionObstacle( std::sort( dynamic_obstacles.begin(), dynamic_obstacles.end(), [&path, &base_pose](const auto & lhs, const auto & rhs) -> bool { - const auto dist_lhs = - motion_utils::calcSignedArcLength(path.points, base_pose.position, lhs.pose.position); - const auto dist_rhs = - motion_utils::calcSignedArcLength(path.points, base_pose.position, rhs.pose.position); + const auto dist_lhs = autoware_motion_utils::calcSignedArcLength( + path.points, base_pose.position, lhs.pose.position); + const auto dist_rhs = autoware_motion_utils::calcSignedArcLength( + path.points, base_pose.position, rhs.pose.position); return dist_lhs < dist_rhs; }); @@ -359,8 +359,8 @@ std::vector RunOutModule::createVehiclePolygon( std::vector RunOutModule::excludeObstaclesOnEgoPath( const std::vector & dynamic_obstacles, const PathWithLaneId & path) { + using autoware_motion_utils::findNearestIndex; using autoware_universe_utils::calcOffsetPose; - using motion_utils::findNearestIndex; if (!planner_param_.run_out.exclude_obstacles_already_in_path || path.points.empty()) { return dynamic_obstacles; } @@ -658,7 +658,7 @@ std::optional RunOutModule::calcStopPoint( } // calculate distance to collision with the obstacle - const float dist_to_collision_point = motion_utils::calcSignedArcLength( + const float dist_to_collision_point = autoware_motion_utils::calcSignedArcLength( path.points, current_pose.position, dynamic_obstacle->nearest_collision_point); const float dist_to_collision = dist_to_collision_point - planner_param_.vehicle_param.base_to_front; @@ -669,7 +669,7 @@ std::optional RunOutModule::calcStopPoint( // calculate the stop point for base link const float base_to_collision_point = planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; - const auto stop_point = motion_utils::calcLongitudinalOffsetPose( + const auto stop_point = autoware_motion_utils::calcLongitudinalOffsetPose( path.points, dynamic_obstacle->nearest_collision_point, -base_to_collision_point, false); if (!stop_point) { RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point."); @@ -691,7 +691,7 @@ std::optional RunOutModule::calcStopPoint( const float planning_dec = jerk_dec < planner_param_.common.normal_min_jerk ? planner_param_.common.limit_min_acc : planner_param_.common.normal_min_acc; - auto stop_dist = motion_utils::calcDecelDistWithJerkAndAccConstraints( + auto stop_dist = autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints( current_vel, target_vel, current_acc, planning_dec, jerk_acc, jerk_dec); if (!stop_dist) { RCLCPP_WARN_STREAM(logger_, "failed to calculate stop distance."); @@ -727,7 +727,7 @@ std::optional RunOutModule::calcStopPoint( // calculate the stop point for base link const float base_to_collision_point = planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; - const auto stop_point = motion_utils::calcLongitudinalOffsetPose( + const auto stop_point = autoware_motion_utils::calcLongitudinalOffsetPose( path.points, dynamic_obstacle->nearest_collision_point, -base_to_collision_point, false); if (!stop_point) { RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point."); @@ -754,7 +754,7 @@ bool RunOutModule::insertStopPoint( // find nearest point index behind the stop point const auto nearest_seg_idx = - motion_utils::findNearestSegmentIndex(path.points, stop_point->position); + autoware_motion_utils::findNearestSegmentIndex(path.points, stop_point->position); auto insert_idx = nearest_seg_idx + 1; // if stop point is ahead of the end of the path, don't insert @@ -788,7 +788,7 @@ void RunOutModule::insertVelocityForState( state_input.current_velocity = current_vel; state_input.current_obstacle = dynamic_obstacle; state_input.dist_to_collision = - motion_utils::calcSignedArcLength( + autoware_motion_utils::calcSignedArcLength( smoothed_path.points, current_pose.position, dynamic_obstacle->nearest_collision_point) - planner_param.vehicle_param.base_to_front; @@ -857,14 +857,14 @@ void RunOutModule::insertApproachingVelocity( { // insert slow down velocity from nearest segment point const auto nearest_seg_idx = - motion_utils::findNearestSegmentIndex(output_path.points, current_pose.position); + autoware_motion_utils::findNearestSegmentIndex(output_path.points, current_pose.position); run_out_utils::insertPathVelocityFromIndexLimited( nearest_seg_idx, approaching_vel, output_path.points); // calculate stop point to insert 0 velocity const float base_to_collision_point = approach_margin + planner_param_.vehicle_param.base_to_front; - const auto stop_point = motion_utils::calcLongitudinalOffsetPose( + const auto stop_point = autoware_motion_utils::calcLongitudinalOffsetPose( output_path.points, dynamic_obstacle.nearest_collision_point, -base_to_collision_point, false); if (!stop_point) { RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point."); @@ -876,7 +876,7 @@ void RunOutModule::insertApproachingVelocity( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); const auto nearest_seg_idx_stop = - motion_utils::findNearestSegmentIndex(output_path.points, stop_point->position); + autoware_motion_utils::findNearestSegmentIndex(output_path.points, stop_point->position); auto insert_idx_stop = nearest_seg_idx_stop + 1; // to PathPointWithLaneId @@ -899,7 +899,7 @@ void RunOutModule::applyMaxJerkLimit( const auto stop_point = path.points.at(stop_point_idx.value()).point.pose.position; const auto dist_to_stop_point = - motion_utils::calcSignedArcLength(path.points, current_pose.position, stop_point); + autoware_motion_utils::calcSignedArcLength(path.points, current_pose.position, stop_point); // calculate desired velocity with limited jerk const auto jerk_limited_vel = planning_utils::calcDecelerationVelocityFromDistanceToTarget( @@ -982,15 +982,15 @@ void RunOutModule::publishDebugValue( const geometry_msgs::msg::Pose & current_pose) const { if (dynamic_obstacle) { - const auto lateral_dist = - std::abs(motion_utils::calcLateralOffset(path.points, dynamic_obstacle->pose.position)) - - planner_param_.vehicle_param.width / 2.0; + const auto lateral_dist = std::abs(autoware_motion_utils::calcLateralOffset( + path.points, dynamic_obstacle->pose.position)) - + planner_param_.vehicle_param.width / 2.0; const auto longitudinal_dist_to_obstacle = - motion_utils::calcSignedArcLength( + autoware_motion_utils::calcSignedArcLength( path.points, current_pose.position, dynamic_obstacle->pose.position) - planner_param_.vehicle_param.base_to_front; - const float dist_to_collision_point = motion_utils::calcSignedArcLength( + const float dist_to_collision_point = autoware_motion_utils::calcSignedArcLength( path.points, current_pose.position, dynamic_obstacle->nearest_collision_point); const auto dist_to_collision = dist_to_collision_point - planner_param_.vehicle_param.base_to_front; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 450788003f45..80404b69232e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -52,7 +52,7 @@ class RunOutModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; void setPlannerParam(const PlannerParam & planner_param); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp index 448fdce82028..3745c01f51dd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp @@ -14,8 +14,8 @@ #include "utils.hpp" -#include -#include +#include +#include #include #include @@ -31,8 +31,8 @@ #else #include #endif +#include #include -#include namespace autoware::behavior_velocity_planner { namespace run_out_utils @@ -218,7 +218,7 @@ std::vector excludeObstaclesOutSideOfLine( std::vector extracted_dynamic_obstacle; for (const auto & obstacle : dynamic_obstacles) { const auto obstacle_nearest_idx = - motion_utils::findNearestIndex(path_points, obstacle.pose.position); + autoware_motion_utils::findNearestIndex(path_points, obstacle.pose.position); const auto & obstacle_nearest_path_point = path_points.at(obstacle_nearest_idx).point.pose.position; @@ -272,7 +272,8 @@ PathWithLaneId trimPathFromSelfPose( const PathWithLaneId & input, const geometry_msgs::msg::Pose & self_pose, const double trim_distance) { - const size_t nearest_idx = motion_utils::findNearestIndex(input.points, self_pose.position); + const size_t nearest_idx = + autoware_motion_utils::findNearestIndex(input.points, self_pose.position); PathWithLaneId output{}; output.header = input.header; @@ -380,7 +381,7 @@ Polygons2d createDetectionAreaPolygon( const float jerk_acc = std::abs(jerk_dec); const float planning_dec = jerk_dec < pp.common.normal_min_jerk ? pp.common.limit_min_acc : pp.common.normal_min_acc; - auto stop_dist = motion_utils::calcDecelDistWithJerkAndAccConstraints( + auto stop_dist = autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints( initial_vel, target_vel, initial_acc, planning_dec, jerk_acc, jerk_dec); if (!stop_dist) { @@ -400,7 +401,7 @@ Polygons2d createDetectionAreaPolygon( da_range.left_overhang = pp.vehicle_param.left_overhang; da_range.max_lateral_distance = obstacle_vel_mps * pp.dynamic_obstacle.max_prediction_time; Polygons2d detection_area_poly; - const size_t ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const size_t ego_seg_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, pd.current_odometry->pose, pd.ego_nearest_dist_threshold, pd.ego_nearest_yaw_threshold); planning_utils::createDetectionAreaPolygons( @@ -424,7 +425,7 @@ Polygons2d createMandatoryDetectionAreaPolygon( const float jerk_acc = std::abs(jerk_dec); const float planning_dec = jerk_dec < pp.common.normal_min_jerk ? pp.common.limit_min_acc : pp.common.normal_min_acc; - auto stop_dist = motion_utils::calcDecelDistWithJerkAndAccConstraints( + auto stop_dist = autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints( initial_vel, target_vel, initial_acc, planning_dec, jerk_acc, jerk_dec); if (!stop_dist) { @@ -442,7 +443,7 @@ Polygons2d createMandatoryDetectionAreaPolygon( da_range.left_overhang = pp.vehicle_param.left_overhang; da_range.max_lateral_distance = obstacle_vel_mps * pp.dynamic_obstacle.max_prediction_time; Polygons2d detection_area_poly; - const size_t ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const size_t ego_seg_idx = autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, pd.current_odometry->pose, pd.ego_nearest_dist_threshold, pd.ego_nearest_yaw_threshold); planning_utils::createDetectionAreaPolygons( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml index f463f89318ba..5a4b89716752 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml @@ -19,12 +19,12 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_planning_msgs autoware_route_handler eigen geometry_msgs lanelet2_extension - motion_utils pluginlib rclcpp tf2_geometry_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp index e33d189ede54..67f149124e3f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp @@ -15,9 +15,9 @@ #include "scene.hpp" #include +#include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -96,14 +96,14 @@ visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray() return debug_marker_array; } -motion_utils::VirtualWalls StopLineModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls StopLineModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWalls virtual_walls; if (debug_data_.stop_pose && (state_ == State::APPROACH || state_ == State::STOPPED)) { - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWall wall; wall.text = "stopline"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; wall.ns = std::to_string(module_id_) + "_"; wall.pose = autoware_universe_utils::calcOffsetPose( *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 458118f9ae1c..e851d0c442dc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include @@ -73,7 +73,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( path->points, stop_pose.position, stop_point_idx); const size_t current_seg_idx = findEgoSegmentIndex(path->points); - const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( + const double signed_arc_dist_to_stop_point = autoware_motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, current_seg_idx, stop_pose.position, stop_line_seg_idx); switch (state_) { @@ -116,7 +116,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop case State::STOPPED: { // Change state after vehicle departure - const auto stopped_pose = motion_utils::calcLongitudinalOffsetPose( + const auto stopped_pose = autoware_motion_utils::calcLongitudinalOffsetPose( path->points, planner_data_->current_odometry->pose.position, 0.0); if (!stopped_pose) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 52c975c2e246..7fc9492229cc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -86,7 +86,7 @@ class StopLineModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; private: std::shared_ptr stopped_time_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md index 130da3f91948..4c90b8dd291c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md @@ -26,7 +26,7 @@ The `TemplateModule` class serves as a foundation for creating a scene module wi #### `createVirtualWalls` Method -- The `createVirtualWalls` method creates virtual walls for the scene and returns them as `motion_utils::VirtualWalls`. In the provided code, it returns an empty `VirtualWalls` object. +- The `createVirtualWalls` method creates virtual walls for the scene and returns them as `autoware_motion_utils::VirtualWalls`. In the provided code, it returns an empty `VirtualWalls` object. - You should implement the logic to create virtual walls based on your module's requirements. ## `Manager` diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml index 49f3ab940d0d..d4a7f35faef5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml @@ -14,6 +14,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_planning_msgs autoware_route_handler autoware_universe_utils @@ -21,7 +22,6 @@ geometry_msgs lanelet2_extension libboost-dev - motion_utils pluginlib rclcpp sensor_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index b664186bb6bb..169381b819c4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" // #include "autoware/universe_utils/autoware_universe_utils.hpp" #include @@ -36,9 +36,9 @@ visualization_msgs::msg::MarkerArray TemplateModule::createDebugMarkerArray() return ma; }; -motion_utils::VirtualWalls TemplateModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls TemplateModule::createVirtualWalls() { - motion_utils::VirtualWalls vw; + autoware_motion_utils::VirtualWalls vw; return vw; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index 3397d7720560..b3eb7f65ba26 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -57,11 +57,11 @@ class TemplateModule : public SceneModuleInterface * @brief Create virtual walls for the scene. * * This method is responsible for generating virtual walls for the scene and returning them as a - * `motion_utils::VirtualWalls` object. + * `autoware_motion_utils::VirtualWalls` object. * - * @return A `motion_utils::VirtualWalls` object representing virtual walls in the scene. + * @return A `autoware_motion_utils::VirtualWalls` object representing virtual walls in the scene. */ - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index d983a4ac2790..e385be9f9b2c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -27,7 +28,6 @@ eigen geometry_msgs lanelet2_extension - motion_utils pluginlib rclcpp tf2 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp index d4af01842c78..8dc7fd12a7bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/debug.cpp @@ -15,9 +15,9 @@ #include "scene.hpp" #include +#include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -32,20 +32,20 @@ visualization_msgs::msg::MarkerArray TrafficLightModule::createDebugMarkerArray( return debug_marker_array; } -motion_utils::VirtualWalls TrafficLightModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls TrafficLightModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; wall.text = "traffic_light"; wall.ns = std::to_string(module_id_) + "_"; - wall.style = motion_utils::VirtualWallType::deadline; + wall.style = autoware_motion_utils::VirtualWallType::deadline; for (const auto & p : debug_data_.dead_line_poses) { wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 3a915651ef87..3d2b651af45f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -15,7 +15,7 @@ #include "scene.hpp" #include -#include +#include #include #include @@ -210,7 +210,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * geometry_msgs::msg::Point stop_line_point_msg; stop_line_point_msg.x = stop_line_point.x(); stop_line_point_msg.y = stop_line_point.y(); - const double signed_arc_length_to_stop_point = motion_utils::calcSignedArcLength( + const double signed_arc_length_to_stop_point = autoware_motion_utils::calcSignedArcLength( input_path.points, self_pose->pose.position, stop_line_point_msg); setDistance(signed_arc_length_to_stop_point); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 72afb4725928..6ac08f555fb2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -74,7 +74,7 @@ class TrafficLightModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; inline TrafficSignal getTrafficSignal() const { return looking_tl_state_; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index b123d64d2eac..690eff973a08 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -18,13 +18,13 @@ autoware_adapi_v1_msgs autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_planning_msgs autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils geometry_msgs lanelet2_extension - motion_utils nlohmann-json-dev pluginlib rclcpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 292680a68838..10ec9a7b961f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -14,10 +14,11 @@ #include "scene.hpp" +#include +#include #include #include -#include -#include +using autoware_motion_utils::createStopVirtualWallMarker; using autoware_universe_utils::appendMarkerArray; using autoware_universe_utils::createDefaultMarker; using autoware_universe_utils::createMarkerColor; @@ -25,7 +26,6 @@ using autoware_universe_utils::createMarkerOrientation; using autoware_universe_utils::createMarkerPosition; using autoware_universe_utils::createMarkerScale; using autoware_universe_utils::toMsg; -using motion_utils::createStopVirtualWallMarker; using namespace std::literals::string_literals; namespace autoware::behavior_velocity_planner @@ -51,13 +51,13 @@ namespace } } // namespace -motion_utils::VirtualWalls VirtualTrafficLightModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls VirtualTrafficLightModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; wall.text = "virtual_traffic_light"; wall.ns = std::to_string(module_id_) + "_"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; const auto & d = module_data_; // virtual_wall_stop_line std::vector wall_poses; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index bb398d961068..9988bbd3f602 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include @@ -158,8 +158,8 @@ std::optional insertStopVelocityAtCollision( const SegmentIndexWithPoint & collision, const double offset, tier4_planning_msgs::msg::PathWithLaneId * path) { - const auto collision_offset = - motion_utils::calcLongitudinalOffsetToSegment(path->points, collision.index, collision.point); + const auto collision_offset = autoware_motion_utils::calcLongitudinalOffsetToSegment( + path->points, collision.index, collision.point); const auto offset_segment = arc_lane_utils::findOffsetSegment(*path, collision.index, offset + collision_offset); @@ -410,7 +410,7 @@ bool VirtualTrafficLightModule::isBeforeStartLine(const size_t end_line_idx) const auto & ego_pose = planner_data_->current_odometry->pose; const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); - const auto signed_arc_length = motion_utils::calcSignedArcLength( + const auto signed_arc_length = autoware_motion_utils::calcSignedArcLength( module_data_.path.points, ego_pose.position, ego_seg_idx, collision->point, collision_seg_idx) - planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -433,7 +433,7 @@ bool VirtualTrafficLightModule::isBeforeStopLine(const size_t end_line_idx) const auto & ego_pose = planner_data_->current_odometry->pose; const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); - const auto signed_arc_length = motion_utils::calcSignedArcLength( + const auto signed_arc_length = autoware_motion_utils::calcSignedArcLength( module_data_.path.points, ego_pose.position, ego_seg_idx, collision->point, collision_seg_idx) - planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -461,7 +461,7 @@ bool VirtualTrafficLightModule::isAfterAnyEndLine(const size_t end_line_idx) const auto & ego_pose = planner_data_->current_odometry->pose; const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); - const auto signed_arc_length = motion_utils::calcSignedArcLength( + const auto signed_arc_length = autoware_motion_utils::calcSignedArcLength( module_data_.path.points, ego_pose.position, ego_seg_idx, collision->point, collision_seg_idx) - planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -482,7 +482,7 @@ bool VirtualTrafficLightModule::isNearAnyEndLine(const size_t end_line_idx) const auto & ego_pose = planner_data_->current_odometry->pose; const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); - const auto signed_arc_length = motion_utils::calcSignedArcLength( + const auto signed_arc_length = autoware_motion_utils::calcSignedArcLength( module_data_.path.points, ego_pose.position, ego_seg_idx, collision->point, collision_seg_idx) - planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -544,7 +544,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( path->points, collision->point, collision->index); const auto stop_distance = - motion_utils::calcSignedArcLength( + autoware_motion_utils::calcSignedArcLength( path->points, ego_pose.position, ego_seg_idx, collision.value().point, collision_seg_idx) + offset; const auto is_stopped = planner_data_->isVehicleStopped(); @@ -552,7 +552,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( if (stop_distance < planner_param_.hold_stop_margin_distance && is_stopped) { SegmentIndexWithPoint new_collision; const auto ego_pos_on_path = - motion_utils::calcLongitudinalOffsetPoint(path->points, ego_pose.position, 0.0); + autoware_motion_utils::calcLongitudinalOffsetPoint(path->points, ego_pose.position, 0.0); if (ego_pos_on_path) { new_collision.point = ego_pos_on_path.value(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 5ffc583067f0..e26c581ae677 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -82,7 +82,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; private: const int64_t lane_id_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml index 27d31f840524..f02ec254d64f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml @@ -19,6 +19,7 @@ autoware_behavior_velocity_crosswalk_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_planning_msgs autoware_route_handler autoware_universe_utils @@ -26,7 +27,6 @@ geometry_msgs lanelet2_extension libboost-dev - motion_utils pluginlib rclcpp visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp index d95b3c54ae59..e575d6c1b7f0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/debug.cpp @@ -15,24 +15,24 @@ #include "scene_walkway.hpp" #include +#include +#include #include #include -#include -#include #include namespace autoware::behavior_velocity_planner { +using autoware_motion_utils::createSlowDownVirtualWallMarker; +using autoware_motion_utils::createStopVirtualWallMarker; using autoware_universe_utils::appendMarkerArray; using autoware_universe_utils::calcOffsetPose; using autoware_universe_utils::createDefaultMarker; using autoware_universe_utils::createMarkerColor; using autoware_universe_utils::createMarkerScale; using autoware_universe_utils::createPoint; -using motion_utils::createSlowDownVirtualWallMarker; -using motion_utils::createStopVirtualWallMarker; using visualization_msgs::msg::Marker; namespace @@ -74,14 +74,14 @@ visualization_msgs::msg::MarkerArray createWalkwayMarkers( } } // namespace -motion_utils::VirtualWalls WalkwayModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls WalkwayModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; wall.text = "walkway"; wall.ns = std::to_string(module_id_) + "_"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; for (const auto & p : debug_data_.stop_poses) { wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 7b008d954155..c876608a93df 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -15,18 +15,18 @@ #include "scene_walkway.hpp" #include -#include +#include #include namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; +using autoware_motion_utils::calcLongitudinalOffsetPose; +using autoware_motion_utils::calcSignedArcLength; +using autoware_motion_utils::findNearestSegmentIndex; using autoware_universe_utils::createPoint; using autoware_universe_utils::getPose; -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestSegmentIndex; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index ef6e1116489d..6ad247720b71 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -49,7 +49,7 @@ class WalkwayModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; private: const int64_t module_id_; diff --git a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/package.xml index e0230fb2fb21..e03a05cfc57c 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/package.xml @@ -20,6 +20,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_planning_msgs autoware_route_handler autoware_universe_utils @@ -27,7 +28,6 @@ geometry_msgs lanelet2_extension libboost-dev - motion_utils pluginlib rclcpp sensor_msgs diff --git a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/debug.cpp index 33a25c82429e..804d9109a7c1 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -15,9 +15,9 @@ #include "scene.hpp" #include +#include #include #include -#include #include @@ -71,18 +71,18 @@ visualization_msgs::msg::MarkerArray createNoDrivableLaneMarkers( } } // namespace -motion_utils::VirtualWalls NoDrivableLaneModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls NoDrivableLaneModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWalls virtual_walls; const auto now = this->clock_->now(); if ( (state_ == State::APPROACHING) || (state_ == State::INSIDE_NO_DRIVABLE_LANE) || (state_ == State::STOPPED)) { - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWall wall; wall.text = "no_drivable_lane"; - wall.style = motion_utils::VirtualWallType::stop; + wall.style = autoware_motion_utils::VirtualWallType::stop; wall.ns = std::to_string(module_id_) + "_"; wall.pose = debug_data_.stop_pose; virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.cpp index ad4c6fc9cf63..cf663d314a3c 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "util.hpp" #include @@ -58,7 +58,7 @@ bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason if (path_no_drivable_lane_polygon_intersection.first_intersection_point) { first_intersection_point = path_no_drivable_lane_polygon_intersection.first_intersection_point.value(); - distance_ego_first_intersection = motion_utils::calcSignedArcLength( + distance_ego_first_intersection = autoware_motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, first_intersection_point); distance_ego_first_intersection -= planner_data_->vehicle_info_.max_longitudinal_offset_m; } @@ -135,7 +135,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR const double longitudinal_offset = -1.0 * (planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m); - const auto op_target_point = motion_utils::calcLongitudinalOffsetPoint( + const auto op_target_point = autoware_motion_utils::calcLongitudinalOffsetPoint( path->points, first_intersection_point, longitudinal_offset); geometry_msgs::msg::Point target_point; @@ -144,10 +144,11 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR target_point = op_target_point.value(); } - const auto target_segment_idx = motion_utils::findNearestSegmentIndex(path->points, target_point); + const auto target_segment_idx = + autoware_motion_utils::findNearestSegmentIndex(path->points, target_point); const auto & op_target_point_idx = - motion_utils::insertTargetPoint(target_segment_idx, target_point, path->points, 5e-2); + autoware_motion_utils::insertTargetPoint(target_segment_idx, target_point, path->points, 5e-2); size_t target_point_idx; if (op_target_point_idx) { target_point_idx = op_target_point_idx.value(); @@ -169,7 +170,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); - const auto virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( + const auto virtual_wall_pose = autoware_motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); debug_data_.stop_pose = virtual_wall_pose.value(); @@ -177,9 +178,9 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR const size_t current_seg_idx = findEgoSegmentIndex(path->points); const auto intersection_segment_idx = - motion_utils::findNearestSegmentIndex(path->points, first_intersection_point); + autoware_motion_utils::findNearestSegmentIndex(path->points, first_intersection_point); const double signed_arc_dist_to_intersection_point = - motion_utils::calcSignedArcLength( + autoware_motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, current_seg_idx, first_intersection_point, intersection_segment_idx) - planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -222,7 +223,7 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); - const auto & virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( + const auto & virtual_wall_pose = autoware_motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); debug_data_.stop_pose = virtual_wall_pose.value(); @@ -239,7 +240,7 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReason * stop_reason) { - const auto & stopped_pose = motion_utils::calcLongitudinalOffsetPose( + const auto & stopped_pose = autoware_motion_utils::calcLongitudinalOffsetPose( path->points, planner_data_->current_odometry->pose.position, 0.0); if (!stopped_pose) { @@ -264,7 +265,7 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReaso velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); - const auto virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( + const auto virtual_wall_pose = autoware_motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); debug_data_.stop_pose = virtual_wall_pose.value(); diff --git a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.hpp index b3cec6c41c0f..d8425bdd2a29 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -60,7 +60,7 @@ class NoDrivableLaneModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; private: const int64_t lane_id_; diff --git a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/util.cpp index afa4a0956021..7aede9a509c7 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -14,11 +14,11 @@ #include "util.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include -#include +#include #include @@ -32,8 +32,8 @@ using Point = bg::model::d2::point_xy; using Polygon = bg::model::polygon; using Line = bg::model::linestring; +using autoware_motion_utils::calcSignedArcLength; using autoware_universe_utils::createPoint; -using motion_utils::calcSignedArcLength; PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLanePolygon( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml index 73d758213111..805ab2f39a7e 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml @@ -16,6 +16,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_planning_msgs autoware_route_handler autoware_universe_utils @@ -23,7 +24,6 @@ geometry_msgs lanelet2_extension libboost-dev - motion_utils pluginlib rclcpp sensor_msgs diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/debug.cpp index 3858490c19f5..fca9d3c2ec79 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/debug.cpp @@ -15,23 +15,23 @@ #include "scene.hpp" #include +#include +#include #include #include #include -#include -#include #include namespace autoware::behavior_velocity_planner { +using autoware_motion_utils::createSlowDownVirtualWallMarker; using autoware_universe_utils::appendMarkerArray; using autoware_universe_utils::calcOffsetPose; using autoware_universe_utils::createDefaultMarker; using autoware_universe_utils::createMarkerColor; using autoware_universe_utils::createMarkerScale; using autoware_universe_utils::createPoint; -using motion_utils::createSlowDownVirtualWallMarker; using visualization_msgs::msg::Marker; namespace @@ -96,13 +96,13 @@ visualization_msgs::msg::MarkerArray createSpeedBumpMarkers( } } // namespace -motion_utils::VirtualWalls SpeedBumpModule::createVirtualWalls() +autoware_motion_utils::VirtualWalls SpeedBumpModule::createVirtualWalls() { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; wall.text = "speed_bump"; wall.ns = std::to_string(module_id_) + "_"; - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware_motion_utils::VirtualWallType::slowdown; for (const auto & p : debug_data_.slow_start_poses) { wall.pose = autoware_universe_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.cpp index 5c4f8a39c021..546aa0369b1f 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.cpp @@ -14,16 +14,16 @@ #include "scene.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "util.hpp" #include namespace autoware::behavior_velocity_planner { +using autoware_motion_utils::calcSignedArcLength; using autoware_universe_utils::createPoint; -using motion_utils::calcSignedArcLength; using geometry_msgs::msg::Point32; diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.hpp index d140aa496133..504cfa243031 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.hpp @@ -59,7 +59,7 @@ class SpeedBumpModule : public SceneModuleInterface bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; + autoware_motion_utils::VirtualWalls createVirtualWalls() override; private: int64_t module_id_; diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.cpp index 41cfc911e298..1e00c0ac9210 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.cpp +++ b/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.cpp @@ -14,8 +14,8 @@ #include "util.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include #include @@ -40,11 +40,11 @@ using Point = bg::model::d2::point_xy; using Polygon = bg::model::polygon; using Line = bg::model::linestring; +using autoware_motion_utils::calcLongitudinalOffsetPoint; +using autoware_motion_utils::calcSignedArcLength; +using autoware_motion_utils::findNearestSegmentIndex; +using autoware_motion_utils::insertTargetPoint; using autoware_universe_utils::createPoint; -using motion_utils::calcLongitudinalOffsetPoint; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestSegmentIndex; -using motion_utils::insertTargetPoint; PathPolygonIntersectionStatus getPathPolygonIntersectionStatus( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml index da2ff14c7681..070843cacb2b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_motion_velocity_planner_common autoware_perception_msgs autoware_planning_msgs @@ -20,7 +21,6 @@ autoware_vehicle_info_utils geometry_msgs libboost-dev - motion_utils pluginlib rclcpp tf2 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp index d5873e5f462a..783c3f880598 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -14,9 +14,9 @@ #include "collision.hpp" +#include #include #include -#include #include @@ -48,8 +48,8 @@ std::optional find_closest_collision_point( object_footprint.outer(), ego_footprint.outer(), collision_points); for (const auto & coll_p : collision_points) { auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y()); - const auto dist_to_ego = - motion_utils::calcSignedArcLength(ego_data.trajectory, ego_data.pose.position, p); + const auto dist_to_ego = autoware_motion_utils::calcSignedArcLength( + ego_data.trajectory, ego_data.pose.position, p); if (dist_to_ego < closest_dist) { closest_dist = dist_to_ego; closest_collision_point = p; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index ee4dbb01bd71..fd8bf467ddc3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -21,12 +21,12 @@ #include "object_stop_decision.hpp" #include "types.hpp" +#include +#include #include #include #include #include -#include -#include #include #include @@ -42,7 +42,7 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo module_name_ = module_name; logger_ = node.get_logger().get_child(ns_); clock_ = node.get_clock(); - velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + velocity_factor_interface_.init(autoware_motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -101,20 +101,20 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( dynamic_obstacle_stop::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; ego_data.trajectory = ego_trajectory_points; - motion_utils::removeOverlapPoints(ego_data.trajectory); + autoware_motion_utils::removeOverlapPoints(ego_data.trajectory); ego_data.first_trajectory_idx = - motion_utils::findNearestSegmentIndex(ego_data.trajectory, ego_data.pose.position); + autoware_motion_utils::findNearestSegmentIndex(ego_data.trajectory, ego_data.pose.position); ego_data.longitudinal_offset_to_first_trajectory_idx = - motion_utils::calcLongitudinalOffsetToSegment( + autoware_motion_utils::calcLongitudinalOffsetToSegment( ego_data.trajectory, ego_data.first_trajectory_idx, ego_data.pose.position); - const auto min_stop_distance = motion_utils::calcDecelDistWithJerkAndAccConstraints( + const auto min_stop_distance = autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints( planner_data->current_odometry.twist.twist.linear.x, 0.0, planner_data->current_acceleration.accel.accel.linear.x, planner_data->velocity_smoother_->getMinDecel(), planner_data->velocity_smoother_->getMaxJerk(), planner_data->velocity_smoother_->getMinJerk()) .value_or(0.0); - ego_data.earliest_stop_pose = motion_utils::calcLongitudinalOffsetPose( + ego_data.earliest_stop_pose = autoware_motion_utils::calcLongitudinalOffsetPose( ego_data.trajectory, ego_data.pose.position, min_stop_distance); dynamic_obstacle_stop::make_ego_footprint_rtree(ego_data, params_); @@ -141,13 +141,13 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( dynamic_obstacle_stop::find_earliest_collision(object_map_, ego_data); const auto collisions_duration_us = stopwatch.toc("collisions"); if (earliest_collision) { - const auto arc_length_diff = motion_utils::calcSignedArcLength( + const auto arc_length_diff = autoware_motion_utils::calcSignedArcLength( ego_data.trajectory, *earliest_collision, ego_data.pose.position); const auto can_stop_before_limit = arc_length_diff < min_stop_distance - params_.ego_longitudinal_offset - params_.stop_distance_buffer; const auto stop_pose = can_stop_before_limit - ? motion_utils::calcLongitudinalOffsetPose( + ? autoware_motion_utils::calcLongitudinalOffsetPose( ego_data.trajectory, *earliest_collision, -params_.stop_distance_buffer - params_.ego_longitudinal_offset) : ego_data.earliest_stop_pose; @@ -159,8 +159,8 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( autoware_universe_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; velocity_factor_interface_.set( ego_trajectory_points, ego_data.pose, *stop_pose, - stop_pose_reached ? motion_utils::VelocityFactor::STOPPED - : motion_utils::VelocityFactor::APPROACHING, + stop_pose_reached ? autoware_motion_utils::VelocityFactor::STOPPED + : autoware_motion_utils::VelocityFactor::APPROACHING, "dynamic_obstacle_stop"); result.velocity_factor = velocity_factor_interface_.get(); create_virtual_walls(); @@ -207,10 +207,10 @@ visualization_msgs::msg::MarkerArray DynamicObstacleStopModule::create_debug_mar void DynamicObstacleStopModule::create_virtual_walls() { if (debug_data_.stop_pose) { - motion_utils::VirtualWall virtual_wall; + autoware_motion_utils::VirtualWall virtual_wall; virtual_wall.text = "dynamic_obstacle_stop"; virtual_wall.longitudinal_offset = params_.ego_longitudinal_offset; - virtual_wall.style = motion_utils::VirtualWallType::stop; + virtual_wall.style = autoware_motion_utils::VirtualWallType::stop; virtual_wall.pose = *debug_data_.stop_pose; virtual_wall_marker_creator.add_virtual_wall(virtual_wall); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp index 24dfaa210473..1843d26a22a2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp @@ -18,9 +18,9 @@ #include "object_stop_decision.hpp" #include "types.hpp" +#include #include #include -#include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 223e7487b36e..d227c2ae5d20 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -16,7 +16,7 @@ #include "types.hpp" -#include +#include #include #include @@ -46,13 +46,13 @@ std::vector filter_predicted_obj }) != o.classification.end(); }; const auto is_in_range = [&](const auto & o) { - const auto distance = std::abs(motion_utils::calcLateralOffset( + const auto distance = std::abs(autoware_motion_utils::calcLateralOffset( ego_data.trajectory, o.kinematics.initial_pose_with_covariance.pose.position)); return distance <= params.minimum_object_distance_from_ego_trajectory + params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + hysteresis; }; const auto is_not_too_close = [&](const auto & o) { - const auto obj_arc_length = motion_utils::calcSignedArcLength( + const auto obj_arc_length = autoware_motion_utils::calcSignedArcLength( ego_data.trajectory, ego_data.pose.position, o.kinematics.initial_pose_with_covariance.pose.position); return obj_arc_length > ego_data.longitudinal_offset_to_first_trajectory_idx + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp index ce5416c12ae7..f60349fbf7b9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp @@ -14,7 +14,7 @@ #include "object_stop_decision.hpp" -#include +#include #include @@ -29,7 +29,7 @@ void update_object_map( if (auto search = object_map.find(collision.object_uuid); search != object_map.end()) { search->second.collision_detected = true; const auto is_closer_collision_point = - motion_utils::calcSignedArcLength( + autoware_motion_utils::calcSignedArcLength( trajectory, search->second.collision_point, collision.point) < 0.0; if (is_closer_collision_point) search->second.collision_point = collision.point; } else { @@ -53,7 +53,7 @@ std::optional find_earliest_collision( double earliest_collision_arc_length = std::numeric_limits::max(); for (auto & [object_uuid, decision] : object_map) { if (decision.should_be_avoided()) { - const auto arc_length = motion_utils::calcSignedArcLength( + const auto arc_length = autoware_motion_utils::calcSignedArcLength( ego_data.trajectory, ego_data.pose.position, decision.collision_point); if (arc_length < earliest_collision_arc_length) { earliest_collision_arc_length = arc_length; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml index 7b6f3c6a49c6..c42c7efb8157 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml @@ -12,6 +12,7 @@ eigen3_cmake_module autoware_grid_map_utils + autoware_motion_utils autoware_motion_velocity_planner_common autoware_perception_msgs autoware_planning_msgs @@ -24,7 +25,6 @@ lanelet2_core lanelet2_extension libboost-dev - motion_utils nav_msgs pcl_ros pluginlib diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index a281826b64ed..21dfadbe11e8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -120,7 +120,7 @@ std::vector calculate_slowd TrajectoryPoints & trajectory, const CollisionChecker & collision_checker, const std::vector & projections, const std::vector & footprints, ProjectionParameters & projection_params, const VelocityParameters & velocity_params, - motion_utils::VirtualWalls & virtual_walls) + autoware_motion_utils::VirtualWalls & virtual_walls) { std::vector slowdown_intervals; double time = 0.0; @@ -150,7 +150,7 @@ std::vector calculate_slowd static_cast(*dist_to_collision - projection_params.extra_length), static_cast(projection_params.duration), velocity_params.min_velocity))); - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWall wall; wall.pose = trajectory_point.pose; virtual_walls.push_back(wall); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index e5cf0decb13d..dc250af55df0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp @@ -122,7 +122,7 @@ std::vector calculate_slowd TrajectoryPoints & trajectory, const CollisionChecker & collision_checker, const std::vector & projections, const std::vector & footprints, ProjectionParameters & projection_params, const VelocityParameters & velocity_params, - motion_utils::VirtualWalls & virtual_walls); + autoware_motion_utils::VirtualWalls & virtual_walls); /// @brief copy the velocity profile of a downsampled trajectory to the original trajectory /// @param[in] downsampled_trajectory downsampled trajectory diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 74b027efd3d1..1f621d935de3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -21,12 +21,12 @@ #include "parameters.hpp" #include "trajectory_preprocessing.hpp" +#include +#include #include #include #include #include -#include -#include #include #include @@ -46,7 +46,7 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string projection_params_ = obstacle_velocity_limiter::ProjectionParameters(node); obstacle_params_ = obstacle_velocity_limiter::ObstacleParameters(node); velocity_params_ = obstacle_velocity_limiter::VelocityParameters(node); - velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + velocity_factor_interface_.init(autoware_motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -136,8 +136,8 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( stopwatch.tic(); VelocityPlanningResult result; stopwatch.tic("preprocessing"); - const auto ego_idx = - motion_utils::findNearestIndex(ego_trajectory_points, planner_data->current_odometry.pose.pose); + const auto ego_idx = autoware_motion_utils::findNearestIndex( + ego_trajectory_points, planner_data->current_odometry.pose.pose); if (!ego_idx) { RCLCPP_WARN_THROTTLE( logger_, *clock_, rcutils_duration_value_t(1000), @@ -183,7 +183,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( obstacle_params_); } const auto obstacles_us = stopwatch.toc("obstacles"); - motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWalls virtual_walls; stopwatch.tic("slowdowns"); result.slowdown_intervals = obstacle_velocity_limiter::calculate_slowdown_intervals( downsampled_traj_points, @@ -195,7 +195,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( for (auto & wall : virtual_walls) { wall.longitudinal_offset = vehicle_front_offset_; wall.text = ns_; - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware_motion_utils::VirtualWallType::slowdown; } virtual_wall_marker_creator.add_virtual_walls(virtual_walls); virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index df17c10cd418..c5f7b79fb0b0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -14,6 +14,7 @@ autoware_cmake + autoware_motion_utils autoware_motion_velocity_planner_common autoware_perception_msgs autoware_planning_msgs @@ -23,7 +24,6 @@ geometry_msgs lanelet2_extension libboost-dev - motion_utils pluginlib rclcpp tf2 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index 97df4e400e4d..8ae3005c03b9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -16,8 +16,8 @@ #include "footprint.hpp" -#include -#include +#include +#include #include @@ -30,7 +30,7 @@ bool can_decelerate( const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel) { // TODO(Maxime): use the library function - const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( + const auto dist_ahead_of_ego = autoware_motion_utils::calcSignedArcLength( ego_data.trajectory_points, ego_data.pose.position, point.pose.position); const auto acc_to_target_vel = (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); @@ -42,14 +42,15 @@ std::optional calculate_last_in_lane_pose( const autoware_universe_utils::Polygon2d & footprint, const std::optional & prev_slowdown_point, const PlannerParam & params) { - const auto from_arc_length = - motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0, ego_data.first_trajectory_idx); - const auto to_arc_length = motion_utils::calcSignedArcLength( + const auto from_arc_length = autoware_motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0, ego_data.first_trajectory_idx); + const auto to_arc_length = autoware_motion_utils::calcSignedArcLength( ego_data.trajectory_points, 0, decision.target_trajectory_idx); TrajectoryPoint interpolated_point; for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { // TODO(Maxime): binary search - interpolated_point.pose = motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); + interpolated_point.pose = + autoware_motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); const auto respect_decel_limit = !params.skip_if_over_max_decel || prev_slowdown_point || can_decelerate(ego_data, interpolated_point, decision.velocity); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index e7554c449d26..3585ed9339f2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -226,14 +226,14 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & return debug_marker_array; } -motion_utils::VirtualWalls create_virtual_walls( +autoware_motion_utils::VirtualWalls create_virtual_walls( const DebugData & debug_data, const PlannerParam & params) { - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; + autoware_motion_utils::VirtualWalls virtual_walls; + autoware_motion_utils::VirtualWall wall; wall.text = "out_of_lane"; wall.longitudinal_offset = params.front_offset; - wall.style = motion_utils::VirtualWallType::slowdown; + wall.style = autoware_motion_utils::VirtualWallType::slowdown; for (const auto & slowdown : debug_data.slowdowns) { wall.pose = slowdown.point.pose; virtual_walls.push_back(wall); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp index 4a5be9dfb07a..02f66a27079a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp @@ -17,14 +17,14 @@ #include "types.hpp" -#include +#include #include namespace autoware::motion_velocity_planner::out_of_lane::debug { visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data); -motion_utils::VirtualWalls create_virtual_walls( +autoware_motion_utils::VirtualWalls create_virtual_walls( const DebugData & debug_data, const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane::debug diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp index 7746f1d9ba79..667bd714a848 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp @@ -14,10 +14,10 @@ #include "decisions.hpp" +#include #include #include #include -#include #include @@ -33,7 +33,7 @@ namespace autoware::motion_velocity_planner::out_of_lane { double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx) { - return motion_utils::calcSignedArcLength( + return autoware_motion_utils::calcSignedArcLength( ego_data.trajectory_points, ego_data.pose.position, ego_data.first_trajectory_idx + target_idx); } @@ -80,17 +80,17 @@ std::optional> object_time_to_range( auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { - const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); + const auto unique_path_points = autoware_motion_utils::removeOverlapPoints(predicted_path.path); if (unique_path_points.size() < 2) continue; const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); const auto enter_point = geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); const auto enter_segment_idx = - motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); - const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment( + autoware_motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); + const auto enter_offset = autoware_motion_utils::calcLongitudinalOffsetToSegment( unique_path_points, enter_segment_idx, enter_point); - const auto enter_lat_dist = - std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); + const auto enter_lat_dist = std::abs( + autoware_motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); const auto enter_segment_length = autoware_universe_utils::calcDistance2d( unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); const auto enter_offset_ratio = enter_offset / enter_segment_length; @@ -100,11 +100,11 @@ std::optional> object_time_to_range( const auto exit_point = geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); const auto exit_segment_idx = - motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); - const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment( + autoware_motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); + const auto exit_offset = autoware_motion_utils::calcLongitudinalOffsetToSegment( unique_path_points, exit_segment_idx, exit_point); - const auto exit_lat_dist = - std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); + const auto exit_lat_dist = std::abs( + autoware_motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); const auto exit_segment_length = autoware_universe_utils::calcDistance2d( unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index ea50946a3226..b864d6d0eb56 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -14,7 +14,7 @@ #include "filter_predicted_objects.hpp" -#include +#include #include #include @@ -114,10 +114,10 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( auto filtered_object = object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); + const auto no_overlap_path = autoware_motion_utils::removeOverlapPoints(predicted_path.path); if (no_overlap_path.size() <= 1) return true; const auto lat_offset_to_current_ego = - std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + std::abs(autoware_motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); const auto is_crossing_ego = lat_offset_to_current_ego <= object.shape.dimensions.y / 2.0 + std::max( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index d4f8e3a33e6e..dfcfd5357562 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -23,11 +23,11 @@ #include "overlapping_range.hpp" #include "types.hpp" +#include +#include #include #include #include -#include -#include #include @@ -50,7 +50,7 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) logger_ = node.get_logger(); clock_ = node.get_clock(); init_parameters(node); - velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + velocity_factor_interface_.init(autoware_motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -157,7 +157,7 @@ VelocityPlanningResult OutOfLaneModule::plan( ego_data.pose = planner_data->current_odometry.pose.pose; ego_data.trajectory_points = ego_trajectory_points; ego_data.first_trajectory_idx = - motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + autoware_motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x; ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); stopwatch.tic("calculate_trajectory_footprints"); @@ -234,9 +234,9 @@ VelocityPlanningResult OutOfLaneModule::plan( if ( point_to_insert && prev_inserted_point_ && prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { - const auto arc_length = motion_utils::calcSignedArcLength( + const auto arc_length = autoware_motion_utils::calcSignedArcLength( ego_trajectory_points, 0LU, point_to_insert->point.pose.position); - const auto prev_arc_length = motion_utils::calcSignedArcLength( + const auto prev_arc_length = autoware_motion_utils::calcSignedArcLength( ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); return prev_arc_length < arc_length; } @@ -244,10 +244,10 @@ VelocityPlanningResult OutOfLaneModule::plan( }(); if (should_use_prev_inserted_point) { // if the trajectory changed the prev point is no longer on the trajectory so we project it - const auto insert_arc_length = motion_utils::calcSignedArcLength( + const auto insert_arc_length = autoware_motion_utils::calcSignedArcLength( ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); prev_inserted_point_->point.pose = - motion_utils::calcInterpolatedPose(ego_trajectory_points, insert_arc_length); + autoware_motion_utils::calcInterpolatedPose(ego_trajectory_points, insert_arc_length); point_to_insert = prev_inserted_point_; } if (point_to_insert) { @@ -261,12 +261,12 @@ VelocityPlanningResult OutOfLaneModule::plan( point_to_insert->point.pose.position, point_to_insert->point.pose.position, point_to_insert->slowdown.velocity); - const auto is_approaching = motion_utils::calcSignedArcLength( + const auto is_approaching = autoware_motion_utils::calcSignedArcLength( ego_trajectory_points, ego_data.pose.position, point_to_insert->point.pose.position) > 0.1 && ego_data.velocity > 0.1; - const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING - : motion_utils::VelocityFactor::STOPPED; + const auto status = is_approaching ? autoware_motion_utils::VelocityFactor::APPROACHING + : autoware_motion_utils::VelocityFactor::STOPPED; velocity_factor_interface_.set( ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane"); result.velocity_factor = velocity_factor_interface_.get(); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index 0897a120b7d8..310a73c04d64 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -17,9 +17,9 @@ #include "types.hpp" +#include #include #include -#include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index d54998d5b8f6..79db6b25e7ce 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -18,7 +18,7 @@ #include "planner_data.hpp" #include "velocity_planning_result.hpp" -#include +#include #include #include @@ -40,11 +40,11 @@ class PluginModuleInterface const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; - motion_utils::VelocityFactorInterface velocity_factor_interface_; + autoware_motion_utils::VelocityFactorInterface velocity_factor_interface_; rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; - motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; + autoware_motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp index bfa88eac1ad3..1288884a3797 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ #define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ -#include +#include #include #include 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 2f29f9a8a8d1..d2a3e305180d 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 @@ -16,6 +16,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -25,7 +26,6 @@ geometry_msgs lanelet2_extension libboost-dev - motion_utils rclcpp tier4_planning_msgs visualization_msgs 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 7d272eedd4a7..c00e0dd85620 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 @@ -18,6 +18,7 @@ rosidl_default_generators autoware_map_msgs + autoware_motion_utils autoware_motion_velocity_planner_common autoware_perception_msgs autoware_planning_msgs @@ -28,7 +29,6 @@ geometry_msgs lanelet2_extension libboost-dev - motion_utils 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 77caa70ce562..76d401a8cfe5 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,13 +14,13 @@ #include "node.hpp" +#include #include #include #include #include #include #include -#include #include #include @@ -283,8 +283,10 @@ void MotionVelocityPlannerNode::insert_stop( autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & stop_point) const { - const auto seg_idx = motion_utils::findNearestSegmentIndex(trajectory.points, stop_point); - const auto insert_idx = motion_utils::insertTargetPoint(seg_idx, stop_point, trajectory.points); + const auto seg_idx = + autoware_motion_utils::findNearestSegmentIndex(trajectory.points, stop_point); + const auto insert_idx = + autoware_motion_utils::insertTargetPoint(seg_idx, stop_point, trajectory.points); if (insert_idx) { for (auto idx = *insert_idx; idx < trajectory.points.size(); ++idx) trajectory.points[idx].longitudinal_velocity_mps = 0.0; @@ -298,13 +300,13 @@ void MotionVelocityPlannerNode::insert_slowdown( const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const { const auto from_seg_idx = - motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.from); - const auto from_insert_idx = - motion_utils::insertTargetPoint(from_seg_idx, slowdown_interval.from, trajectory.points); + autoware_motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.from); + const auto from_insert_idx = autoware_motion_utils::insertTargetPoint( + from_seg_idx, slowdown_interval.from, trajectory.points); const auto to_seg_idx = - motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.to); + autoware_motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.to); const auto to_insert_idx = - motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); + autoware_motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); if (from_insert_idx && to_insert_idx) { for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) trajectory.points[idx].longitudinal_velocity_mps = 0.0; @@ -333,9 +335,10 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s auto traj_resampled = smoother->resampleTrajectory( traj_steering_rate_limited, v0, current_pose, planner_data.ego_nearest_dist_threshold, planner_data.ego_nearest_yaw_threshold); - const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( - traj_resampled, current_pose, planner_data.ego_nearest_dist_threshold, - planner_data.ego_nearest_yaw_threshold); + const size_t traj_resampled_closest = + autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_resampled, current_pose, planner_data.ego_nearest_dist_threshold, + planner_data.ego_nearest_yaw_threshold); std::vector debug_trajectories; // Clip trajectory from closest point autoware::motion_velocity_planner::TrajectoryPoints clipped; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 7bcaf44bbc44..2a50cfd2e24e 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -21,11 +21,11 @@ #include "obstacle_stop_planner/debug_marker.hpp" #include "obstacle_stop_planner/planner_data.hpp" +#include +#include #include #include #include -#include -#include #include #include diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index 4d68dbfac2b8..5aca5efb3638 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -21,13 +21,13 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils diagnostic_msgs - motion_utils nav_msgs pcl_conversions pcl_ros diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index b2ccf896619d..1f01bf441731 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -14,7 +14,7 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" -#include "motion_utils/trajectory/trajectory.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include #include @@ -366,7 +366,8 @@ void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( /* get total distance to collision point */ double dist_to_point = 0; // get distance from self to next nearest point - dist_to_point += motion_utils::calcSignedArcLength(trajectory, self_pose.position, size_t(1)); + dist_to_point += + autoware_motion_utils::calcSignedArcLength(trajectory, self_pose.position, size_t(1)); // add distance from next self-nearest-point(=idx:0) to prev point of nearest_point_idx for (int i = 1; i < nearest_point_idx - 1; i++) { @@ -685,7 +686,7 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( if (output_trajectory->size() > 1) { dist_to_first_point = - motion_utils::calcSignedArcLength(*output_trajectory, self_pose.position, size_t(1)); + autoware_motion_utils::calcSignedArcLength(*output_trajectory, self_pose.position, size_t(1)); } double margin_to_insert = dist_to_collision_point * param_.margin_rate_to_change_vel; diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index d105777b49f9..835417f33a4c 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -14,9 +14,9 @@ #include "obstacle_stop_planner/debug_marker.hpp" +#include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -27,16 +27,16 @@ #include #include +using autoware_motion_utils::createDeletedSlowDownVirtualWallMarker; +using autoware_motion_utils::createDeletedStopVirtualWallMarker; +using autoware_motion_utils::createSlowDownVirtualWallMarker; +using autoware_motion_utils::createStopVirtualWallMarker; using autoware_universe_utils::appendMarkerArray; using autoware_universe_utils::calcOffsetPose; using autoware_universe_utils::createDefaultMarker; using autoware_universe_utils::createMarkerColor; using autoware_universe_utils::createMarkerScale; using autoware_universe_utils::createPoint; -using motion_utils::createDeletedSlowDownVirtualWallMarker; -using motion_utils::createDeletedStopVirtualWallMarker; -using motion_utils::createSlowDownVirtualWallMarker; -using motion_utils::createStopVirtualWallMarker; namespace motion_planning { diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 245ed69eb04f..db39b3124145 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -40,17 +40,17 @@ namespace motion_planning { +using autoware_motion_utils::calcLongitudinalOffsetPose; +using autoware_motion_utils::calcLongitudinalOffsetToSegment; +using autoware_motion_utils::calcSignedArcLength; +using autoware_motion_utils::findFirstNearestIndexWithSoftConstraints; +using autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using autoware_perception_msgs::msg::PredictedObject; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::createPoint; using autoware_universe_utils::getPoint; using autoware_universe_utils::getPose; using autoware_universe_utils::getRPY; -using motion_utils::calcLongitudinalOffsetPose; -using motion_utils::calcLongitudinalOffsetToSegment; -using motion_utils::calcSignedArcLength; -using motion_utils::findFirstNearestIndexWithSoftConstraints; -using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_stop_planner", node_options) @@ -326,7 +326,8 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m const auto current_acc = current_acceleration_ptr->accel.accel.linear.x; // TODO(someone): support backward path - const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(input_msg->points); + const auto is_driving_forward = + autoware_motion_utils::isDrivingForwardWithTwist(input_msg->points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; if (!is_driving_forward_) { RCLCPP_WARN_THROTTLE( @@ -342,11 +343,11 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m Trajectory output_trajectory = *input_msg; TrajectoryPoints output_trajectory_points = - motion_utils::convertToTrajectoryPointArray(*input_msg); + autoware_motion_utils::convertToTrajectoryPointArray(*input_msg); // trim trajectory from self pose TrajectoryPoints base_trajectory = trimTrajectoryWithIndexFromSelfPose( - motion_utils::convertToTrajectoryPointArray(*input_msg), planner_data.current_pose, + autoware_motion_utils::convertToTrajectoryPointArray(*input_msg), planner_data.current_pose, planner_data.trajectory_trim_index); // extend trajectory to consider obstacles after the goal @@ -378,7 +379,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m resetExternalVelocityLimit(current_acc, current_vel); } - auto trajectory = motion_utils::convertToTrajectory(output_trajectory_points); + auto trajectory = autoware_motion_utils::convertToTrajectory(output_trajectory_points); publishDebugData(planner_data, current_acc, current_vel); trajectory.header = input_msg->header; @@ -1556,7 +1557,7 @@ void ObstacleStopPlannerNode::filterObstacles( // Check is it near to trajectory const double max_length = calcObstacleMaxLength(object.shape); - const size_t seg_idx = motion_utils::findNearestSegmentIndex( + const size_t seg_idx = autoware_motion_utils::findNearestSegmentIndex( traj, object.kinematics.initial_pose_with_covariance.pose.position); const auto p_front = autoware_universe_utils::getPoint(traj.at(seg_idx)); const auto p_back = autoware_universe_utils::getPoint(traj.at(seg_idx + 1)); diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index ed1ee450140a..3a4839aed7a3 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -14,10 +14,10 @@ #include "obstacle_stop_planner/planner_utils.hpp" +#include +#include +#include #include -#include -#include -#include #include @@ -32,13 +32,13 @@ namespace motion_planning { +using autoware_motion_utils::calcDecelDistWithJerkAndAccConstraints; +using autoware_motion_utils::findFirstNearestIndexWithSoftConstraints; +using autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_universe_utils::calcDistance2d; using autoware_universe_utils::getRPY; -using motion_utils::calcDecelDistWithJerkAndAccConstraints; -using motion_utils::findFirstNearestIndexWithSoftConstraints; -using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; std::optional> calcFeasibleMarginAndVelocity( const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp index 004739c7c9ec..51b28e7b3f2d 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ #define AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" @@ -22,7 +23,6 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp index 70909231de17..b3838ad8c44f 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_sampler_common/structures.hpp" @@ -22,7 +23,6 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -47,7 +47,7 @@ std::optional getPointIndexAfter( } double sum_length = - -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + -autoware_motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); // search forward if (sum_length < offset) { @@ -126,7 +126,7 @@ size_t findEgoIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return motion_utils::findFirstNearestIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -135,7 +135,7 @@ size_t findEgoSegmentIndex( const std::vector & points, const geometry_msgs::msg::Pose & ego_pose, const EgoNearestParam & ego_nearest_param) { - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + return autoware_motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } @@ -158,7 +158,7 @@ std::optional updateFrontPointForFix( // check if the points_for_fix is longer in front than points const double lon_offset_to_prev_front = - motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); + autoware_motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_WARN( rclcpp::get_logger("autoware_path_sampler.trajectory_utils"), diff --git a/planning/sampling_based_planner/autoware_path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml index 48fc6ae8d4b1..26f85631d569 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -15,13 +15,13 @@ autoware_bezier_sampler autoware_frenet_planner + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils geometry_msgs interpolation - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp index 5e59cd977b65..cd67a0726094 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -14,6 +14,8 @@ #include "autoware_path_sampler/node.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware_path_sampler/path_generation.hpp" #include "autoware_path_sampler/prepare_inputs.hpp" #include "autoware_path_sampler/utils/geometry_utils.hpp" @@ -21,8 +23,6 @@ #include "autoware_sampler_common/constraints/hard_constraint.hpp" #include "autoware_sampler_common/constraints/soft_constraint.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "rclcpp/time.hpp" #include @@ -239,7 +239,7 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr) // 0. return if path is backward // TODO(Maxime): support backward path - if (!motion_utils::isDrivingForward(path_ptr->points)) { + if (!autoware_motion_utils::isDrivingForward(path_ptr->points)) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), 5000, "Backward path is NOT supported. Just converting path to trajectory"); @@ -251,12 +251,13 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr) // 3. extend trajectory to connect the optimized trajectory and the following path smoothly if (!generated_traj_points.empty()) { const auto output_traj_msg = - motion_utils::convertToTrajectory(generated_traj_points, path_ptr->header); + autoware_motion_utils::convertToTrajectory(generated_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); } else { auto stopping_traj = trajectory_utils::convertToTrajectoryPoints(planner_data.traj_points); for (auto & p : stopping_traj) p.longitudinal_velocity_mps = 0.0; - const auto output_traj_msg = motion_utils::convertToTrajectory(stopping_traj, path_ptr->header); + const auto output_traj_msg = + autoware_motion_utils::convertToTrajectory(stopping_traj, path_ptr->header); traj_pub_->publish(output_traj_msg); } @@ -532,7 +533,7 @@ void PathSampler::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) { time_keeper_ptr_->tic(__func__); - const auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker( + const auto virtual_wall_marker = autoware_motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); virtual_wall_pub_->publish(virtual_wall_marker); diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp index e5d90521eb94..2676d223d9ff 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp @@ -14,9 +14,9 @@ #include "autoware_path_sampler/utils/trajectory_utils.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware_path_sampler/utils/geometry_utils.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" #include "tf2/utils.h" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -63,10 +63,10 @@ std::vector resampleTrajectoryPoints( { constexpr bool enable_resampling_stop_point = true; - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory( + const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware_motion_utils::resampleTrajectory( traj, interval, false, true, true, enable_resampling_stop_point); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware_motion_utils::convertToTrajectoryPointArray(resampled_traj); } // NOTE: stop point will not be resampled @@ -75,17 +75,17 @@ std::vector resampleTrajectoryPointsWithoutStopPoint( { constexpr bool enable_resampling_stop_point = false; - const auto traj = motion_utils::convertToTrajectory(traj_points); - const auto resampled_traj = motion_utils::resampleTrajectory( + const auto traj = autoware_motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware_motion_utils::resampleTrajectory( traj, interval, false, true, true, enable_resampling_stop_point); - return motion_utils::convertToTrajectoryPointArray(resampled_traj); + return autoware_motion_utils::convertToTrajectoryPointArray(resampled_traj); } void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx) { - const double offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + const double offset_to_segment = autoware_motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); const auto traj_spline = SplineInterpolationPoints2d(traj_points); diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 95ac530eaf42..b95a9f2cefde 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -17,6 +17,7 @@ autoware_control_msgs autoware_map_msgs + autoware_motion_utils autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils @@ -25,7 +26,6 @@ lanelet2_core lanelet2_extension learning_based_vehicle_model - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index bcad2d03cac4..a6c5a1eae4f7 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -14,11 +14,11 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" @@ -333,7 +333,7 @@ double SimplePlanningSimulator::calculate_ego_pitch() const } const auto centerline_points = convert_centerline_to_points(ego_lanelet); const size_t ego_seg_idx = - motion_utils::findNearestSegmentIndex(centerline_points, ego_pose.position); + autoware_motion_utils::findNearestSegmentIndex(centerline_points, ego_pose.position); const auto & prev_point = centerline_points.at(ego_seg_idx); const auto & next_point = centerline_points.at(ego_seg_idx + 1); diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 315fdea27a3f..95ced648f132 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -15,6 +15,7 @@ autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs + autoware_motion_utils autoware_planning_msgs autoware_system_msgs autoware_vehicle_info_utils @@ -24,7 +25,6 @@ diagnostic_graph_utils geographic_msgs geography_utils - motion_utils nav_msgs rclcpp rclcpp_components diff --git a/system/default_ad_api/src/motion.hpp b/system/default_ad_api/src/motion.hpp index 00c911da6bd2..25949adbc39d 100644 --- a/system/default_ad_api/src/motion.hpp +++ b/system/default_ad_api/src/motion.hpp @@ -15,11 +15,11 @@ #ifndef MOTION_HPP_ #define MOTION_HPP_ +#include #include #include #include #include -#include #include // This file should be included after messages. @@ -34,7 +34,7 @@ class MotionNode : public rclcpp::Node explicit MotionNode(const rclcpp::NodeOptions & options); private: - motion_utils::VehicleStopChecker vehicle_stop_checker_; + autoware_motion_utils::VehicleStopChecker vehicle_stop_checker_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::CallbackGroup::SharedPtr group_cli_; Srv srv_accept_; diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index c40bfac40a13..6fd27c4963f6 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -14,7 +14,7 @@ #include "planning.hpp" -#include +#include #include @@ -136,7 +136,8 @@ void PlanningNode::on_timer() const auto & curr_point = kinematic_state_->pose.pose.position; const auto & stop_point = factor.pose.position; const auto & points = trajectory_->points; - factor.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); + factor.distance = + autoware_motion_utils::calcSignedArcLength(points, curr_point, stop_point); } } } diff --git a/system/default_ad_api/src/planning.hpp b/system/default_ad_api/src/planning.hpp index b533390dcc6f..7e754efc4855 100644 --- a/system/default_ad_api/src/planning.hpp +++ b/system/default_ad_api/src/planning.hpp @@ -15,10 +15,10 @@ #ifndef PLANNING_HPP_ #define PLANNING_HPP_ +#include #include #include #include -#include #include #include @@ -48,7 +48,7 @@ class PlanningNode : public rclcpp::Node std::vector steering_factors_; rclcpp::TimerBase::SharedPtr timer_; - using VehicleStopChecker = motion_utils::VehicleStopCheckerBase; + using VehicleStopChecker = autoware_motion_utils::VehicleStopCheckerBase; using Trajectory = planning_interface::Trajectory::Message; using KinematicState = localization_interface::KinematicState::Message; void on_trajectory(const Trajectory::ConstSharedPtr msg); diff --git a/tools/reaction_analyzer/include/subscriber.hpp b/tools/reaction_analyzer/include/subscriber.hpp index 21d370a68b55..ff251251373e 100644 --- a/tools/reaction_analyzer/include/subscriber.hpp +++ b/tools/reaction_analyzer/include/subscriber.hpp @@ -14,7 +14,7 @@ #ifndef SUBSCRIBER_HPP_ #define SUBSCRIBER_HPP_ -#include +#include #include #include #include diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml index 7d5c8b61374d..04de3986e3a7 100644 --- a/tools/reaction_analyzer/package.xml +++ b/tools/reaction_analyzer/package.xml @@ -17,6 +17,7 @@ autoware_adapi_v1_msgs autoware_control_msgs autoware_internal_msgs + autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_system_msgs @@ -25,7 +26,6 @@ eigen libpcl-all-dev message_filters - motion_utils pcl_conversions pcl_ros rclcpp diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp index 9064f42377d1..b58c80c67f67 100644 --- a/tools/reaction_analyzer/src/subscriber.cpp +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -260,13 +260,13 @@ void SubscriberBase::on_trajectory( return; } - const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex( + const auto nearest_seg_idx = autoware_motion_utils::findNearestSegmentIndex( msg_ptr->points, current_odometry_ptr->pose.pose.position); const auto nearest_objects_seg_idx = - motion_utils::findNearestIndex(msg_ptr->points, entity_pose_.position); + autoware_motion_utils::findNearestIndex(msg_ptr->points, entity_pose_.position); - const auto zero_vel_idx = motion_utils::searchZeroVelocityIndex( + const auto zero_vel_idx = autoware_motion_utils::searchZeroVelocityIndex( msg_ptr->points, nearest_seg_idx, nearest_objects_seg_idx); if (zero_vel_idx) { @@ -299,7 +299,7 @@ void SubscriberBase::on_trajectory( return; } - const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex( + const auto nearest_seg_idx = autoware_motion_utils::findNearestSegmentIndex( msg_ptr->points, current_odometry_ptr->pose.pose.position); // find the target index which we will search for zero velocity @@ -310,7 +310,7 @@ void SubscriberBase::on_trajectory( } const auto target_idx = tmp_target_idx; const auto zero_vel_idx = - motion_utils::searchZeroVelocityIndex(msg_ptr->points, nearest_seg_idx, target_idx); + autoware_motion_utils::searchZeroVelocityIndex(msg_ptr->points, nearest_seg_idx, target_idx); if (zero_vel_idx) { RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); diff --git a/vehicle/autoware_accel_brake_map_calibrator/package.xml b/vehicle/autoware_accel_brake_map_calibrator/package.xml index 97e79f921a41..4b246a2234a6 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/package.xml +++ b/vehicle/autoware_accel_brake_map_calibrator/package.xml @@ -14,12 +14,12 @@ ament_cmake_auto autoware_cmake + autoware_motion_utils autoware_raw_vehicle_cmd_converter autoware_universe_utils autoware_vehicle_msgs diagnostic_updater geometry_msgs - motion_utils rclcpp std_msgs std_srvs